How to program Gazebo using Python?
I want to know if I can use this example for programing another humanoid robot
http://gazebosim.org/wiki/Tutorials/drcsim/2.7/ros_animated_robot
#!/usr/bin/env python
import roslib; roslib.load_manifest('joint_animation_tutorial')
import rospy, math, time
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
def jointTrajectoryCommand():
# Initialize the node
rospy.init_node('joint_control')
print rospy.get_rostime().to_sec()
while rospy.get_rostime().to_sec() == 0.0:
time.sleep(0.1)
print rospy.get_rostime().to_sec()
pub = rospy.Publisher('/joint_trajectory', JointTrajectory)
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.header.frame_id = "atlas::pelvis"
jt.joint_names.append("atlas::back_lbz" )
jt.joint_names.append("atlas::back_mby" )
jt.joint_names.append("atlas::back_ubx" )
jt.joint_names.append("atlas::neck_ay" )
jt.joint_names.append("atlas::l_leg_uhz")
jt.joint_names.append("atlas::l_leg_mhx")
jt.joint_names.append("atlas::l_leg_lhy")
jt.joint_names.append("atlas::l_leg_kny")
jt.joint_names.append("atlas::l_leg_uay")
jt.joint_names.append("atlas::l_leg_lax")
jt.joint_names.append("atlas::r_leg_lax")
jt.joint_names.append("atlas::r_leg_uay")
jt.joint_names.append("atlas::r_leg_kny")
jt.joint_names.append("atlas::r_leg_lhy")
jt.joint_names.append("atlas::r_leg_mhx")
jt.joint_names.append("atlas::r_leg_uhz")
jt.joint_names.append("atlas::l_arm_elx")
jt.joint_names.append("atlas::l_arm_ely")
jt.joint_names.append("atlas::l_arm_mwx")
jt.joint_names.append("atlas::l_arm_shx")
jt.joint_names.append("atlas::l_arm_usy")
jt.joint_names.append("atlas::l_arm_uwy")
jt.joint_names.append("atlas::r_arm_elx")
jt.joint_names.append("atlas::r_arm_ely")
jt.joint_names.append("atlas::r_arm_mwx")
jt.joint_names.append("atlas::r_arm_shx")
jt.joint_names.append("atlas::r_arm_usy")
jt.joint_names.append("atlas::r_arm_uwy")
n = 1500
dt = 0.01
rps = 0.05
for i in range (n):
p = JointTrajectoryPoint()
theta = rps*2.0*math.pi*i*dt
x1 = -0.5*math.sin(2*theta)
x2 = 0.5*math.sin(1*theta)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x1)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x1)
p.positions.append(x2)
p.positions.append(x1)
p.positions.append(x1)
jt.points.append(p)
# set duration
jt.points[i].time_from_start = rospy.Duration.from_sec(dt)
rospy.loginfo("test: angles[%d][%f, %f]",n,x1,x2)
pub.publish(jt)
rospy.spin()
if __name__ == '__main__':
try:
jointTrajectoryCommand()
except rospy.ROSInterruptException: pass
Now,
I edit for use with Nao:
#!/usr/bin/env python
import roslib; roslib.load_manifest('nao1')
import rospy, math, time
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
def jointTrajectoryCommand():
# Initialize the node
rospy.init_node('joint_control')
print rospy.get_rostime().to_sec()
while rospy.get_rostime().to_sec() == 0.0:
time.sleep(0.1)
print rospy.get_rostime().to_sec()
pub = rospy.Publisher('/joint_trajectory', JointTrajectory)
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.header.frame_id = "Nao::base_link"
jt.joint_names.append("Nao::Head" )
jt.joint_names.append("Nao::Neck" )
jt.joint_names.append("Nao::LPelvis" )
jt.joint_names.append("Nao::LHip" )
jt.joint_names.append("Nao::LThigh")
jt.joint_names.append("Nao::LTibia")
jt.joint_names.append("Nao::LAnkle")
jt.joint_names.append("Nao::LFeet")
jt.joint_names.append("Nao::LShoulder")
jt.joint_names.append("Nao::LBicep")
jt.joint_names.append("Nao::LElbow")
jt.joint_names.append("Nao::LForeArm")
jt.joint_names.append("Nao::LHand")
jt.joint_names.append("Nao::RPelvis")
jt.joint_names.append("Nao::RHip" )
jt.joint_names.append("Nao::RThigh")
jt.joint_names.append("Nao::RTibia")
jt.joint_names.append("Nao::RAnkle")
jt.joint_names.append("Nao::RFeet")
jt.joint_names.append("Nao::RShoulder")
jt.joint_names.append("Nao::RBicep")
jt.joint_names.append("Nao::RElbow")
jt.joint_names.append("Nao::RForeArm")
jt.joint_names.append("Nao::RHand")
n = 1500
dt = 0.01
rps = 0.05
for i in range (n):
p = JointTrajectoryPoint()
theta = rps*2.0*math.pi*i*dt
x1 = -0.5*math.sin(2*theta)
x2 = 0.5*math.sin(1*theta)
p.positions.append(x3)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x3)
p.positions.append(x2)
p.positions.append(x3)
p.positions.append(x3)
p.positions.append(x2)
p.positions.append(x3)
p.positions.append(x2)
p.positions.append(x3)
p.positions.append(x3)
p.positions.append(x2)
p.positions.append(x2)
p.positions.append(x3)
p.positions.append(x3)
p.positions.append(x3)
p.positions.append(x3)
p.positions.append(x4)
p.positions.append(x2)
p.positions.append(x3)
jt.points.append(p)
# set duration
jt.points[i].time_from_start = rospy.Duration.from_sec(dt)
rospy.loginfo("test: angles[%d][%f, %f]",n,x1,x2,x3,x4)
pub.publish(jt)
rospy.spin()
if __name__ == '__main__':
try:
jointTrajectoryCommand()
except rospy.ROSInterruptException: pass
But,
The robot no move while in te terminal get:
rosrun nao1 joint_animation.py
Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.
Asked by vncntmh on 2014-02-06 22:20:41 UTC
Answers
The tutorial you linked says that the robot (sdf) is provided of a model plugin that subscribes to the topic, and plays back the the velocities. Here is the source of the plugin https://bitbucket.org/osrf/drcsim/src/4dd60578a573/plugins/ros/gazebo_ros_joint_trajectory.cpp?at=default
Now, you have to be sure that you are writing/reading the same topic. Is such plugin contained in the sdf the Nao robot you are using?
Asked by Andrea on 2014-02-09 10:27:35 UTC
Comments
Really, I'm not sure, how I can see this? in the sdf the NAO?
And how I could integrate this pluging with my robot?
Asked by vncntmh on 2014-02-09 21:14:33 UTC
Yes everything is written in the NAO's sdf. It seems to me that you do not have a clear idea of how the interaction Gazebo - ROS works, if I were you I would go through the tutorials. here some links: A general overview on plugins: http://gazebosim.org/wiki/Tutorials/1.9/plugins/overview Model plugins: http://gazebosim.org/wiki/Tutorials/1.9/ROS_Motor_and_Sensor_Plugins A template: https://code.ros.org/svn/ros-pkg/stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_template.cpp
Asked by Andrea on 2014-02-10 02:56:04 UTC
Comments