I figured out a way to do it, here is the code
#!/usr/bin/env python
import sys
import rospy
import std_msgs
import time
import math
import matplotlib.pyplot as plt
from forces_torques.srv import *
def apply_body_wrench_client(body_name, reference_frame, reference_point, wrench, \
start_time, duration):
rospy.wait_for_service('/gazebo/apply_body_wrench')
try:
apply_body_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench)
apply_body_wrench(body_name, reference_frame, reference_point, wrench, \
start_time, duration)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def clear_body_wrench_client(body_name):
rospy.wait_for_service('gazebo/clear_body_wrenches')
try:
clear_body_wrench = rospy.ServiceProxy('gazebo/clear_body_wrenches', BodyRequest)
clear_body_wrench(body_name)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
if __name__ == "__main__":
beginning = time.time()
update_rate = 0.01
period = 1
body_name = 'panda::panda_link7'
reference_frame = 'panda::panda_link7'
start_time = rospy.Time(secs = 0, nsecs = 0)
duration = rospy.Duration(secs = update_rate, nsecs = 0)
reference_point = geometry_msgs.msg.Point(x = 0, y = 0, z = 0)
t = []
f = []
while time.time()-beginning <= 10:
wrench = geometry_msgs.msg.Wrench(force = geometry_msgs.msg.Vector3( x = 0, \
y = 400*math.cos(2*math.pi*(time.time()-beginning)/period), z = 0), \
torque = geometry_msgs.msg.Vector3(x = 0, y = 0, z = 0))
t.append(time.time() - beginning)
f.append(wrench.force.y)
clear_body_wrench_client(body_name)
apply_body_wrench_client(body_name, reference_frame, reference_point, wrench, \
start_time, duration)
time.sleep(update_rate - ((time.time()-beginning)%update_rate))
plt.plot(t, f, drawstyle = "steps-post")
plt.show()
Actually, I'm not sure if it is the best way to do this. As far as I read in other tutorials the time.time() function outputs the number of seconds elapsed since midnight 01/01/1970, and the output number has only two digits, which also have a limited precision depending on your operating system. In fact if I use 0.001 as update rate the node doesn't work any more. Whereas if I put 0.01 it works well. But what if I wanted a higher precision? Is there a better way to do it?
No, you can't. That is not what these tools are for.
And it may make more sense to actually use a node to calculate your set points and then use an actual controller running against your virtual hw to take the robot's state to the desired state.
Thanks gvdhoorn.
About your suggestion: what I would like to obtain is different actually. I would like to simulate an external force acting on the robot end effector. So the force does not come from a controller internal to the robot which is designed to take the robot to a designed state. Therefore implementing a controller does not seem to me the right solution for my case.
To specify better, let's consider a very simple case. I have an rrbot robot, without controllers implemented for the joints, I just spawned in Gazebo the urdf model. I would like to see what happens if I apply a force to its end effector, and I want this force to be variable in time. Is there a way to do this in Gazebo? Using the /gazebo/apply_body_wrench service was just an idea. If this can be done using another tools it's perfectly fine.
That does indeed not sound like something you'd use a controller for, but it's still not something
rosservicecan do for you.What would be the problem with writing a 10 line Python node for this?
re: can this be done with Gazebo: please ask that question over at
answers.gazebosim.org.Using a Python node for this would be great, just I don't know how to do this ;) I'm a very beginner to ROS and Gazebo. I searched some online tutorials and ROS answers website but I didn't find anything that fits my case. I did these tutorials, but I cannot figure out how to apply these things to my case.
Should I write a service node? And in that case should I use the /gazebo/apply_body_wrench service? And should I also write the client node?
By the way, when I wrote "can this be done in Gazebo?" I was inaccurate. What I meant was: is there any way, in ROS or in Gazebo, to do this and then see the results in Gazebo. What I want from Gazebo is just that it shows me the animation of what happens when I apply my sinusoidal force.
Gazebo already runs the service server, you just need to write a service client. Refer to Writing a Simple Service and Client (Python) for an example.
Ok, at a first stage I implemented the node in the following way:
(maximum number of characters reached, continues in the following comment...)
(... continues)
However, in this way I have obtained from a python script what I could already obtain from terminal command.
Now the problem is again how to make the force time-variable. What I thought is to use a loop inside the node. I could compute every e.g. 10 ns a force value according to a sinusoidal law and then apply this force for 10 ns. When 10 ns have passed do this again and so on... But I know using loops is generally bad in Python. Would it be ok in this context? And in case it's ok, could you give me any ...(more)
why?
That's a rather odd statement.