Robotics StackExchange | Archived questions

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

Comments

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