def main():
rospy.init_node('armcommand', anonymous=True)
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command', JointVelocities, queue_size=10)
rospy.sleep(2) #wait for connection
rate = rospy.Rate(70) # 10hz
#initialise positions
jp = JointPositions()
joints = []
for i in range(0, 5):
joints.append(JointValue())
joints[i].joint_uri = "arm_joint_" + str(i+1)
joints[i].unit = "rad"
#initialise velocities
jv = JointVelocities()
vels = []
for i in range(0, 5):
vels.append(JointValue())
vels[i].joint_uri = "arm_joint_" + str(i+1)
vels[i].unit = "s^-1 rad"
#go to home position
for i in range(0,5):
joints[i].value = 0.05
joints[2].value = -joints[2].value
jp.positions = joints;
pubPos.publish(jp)
rospy.sleep(5)
#start main movement
j = 1
while not rospy.is_shutdown():
for i in range(0,5):
joints[i].value = 0.008*j
for i in range(0,5):
vels[i].value = 0.0001*j + abs(random.random() * 0.05)
joints[2].value = -joints[2].value
vels[2].value = -vels[2].value
jp.positions = joints;
jv.velocities = vels;
#pubPos.publish(jp)
pubVel.publish(jv)
j = j+1
rate.sleep()
def joint_velocities(joint_velocities):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
joint_velocity_1 = joint_velocities[0]
joint_velocity_2 = joint_velocities[1]
joint_velocity_3 = joint_velocities[2]
joint_velocity_4 = joint_velocities[3]
joint_velocity_5 = joint_velocities[4]
pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities)
rospy.sleep(0.5)
try:
jv = JointVelocities()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = joint_velocity_1
jv1.unit = "s^-1 rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = joint_velocity_2
jv2.unit = "s^-1 rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = joint_velocity_3
jv3.unit = "s^-1 rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = joint_velocity_4
jv4.unit = "s^-1 rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = joint_velocity_5
jv5.unit = "s^-1 rad"
p = Poison()
#print p
jv.poisonStamp = p
jv.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jv)
return 'arm moved successfully'
except Exception, e:
print e
return 'arm move failure'
def publish_arm_joint_velocities(self, Joint_Velocities):
desiredVelocities = JointVelocities()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i + 1)
joint.unit = "s^-1 rad"
joint.value = Joint_Velocities[i]
jointCommands.append(joint)
desiredVelocities.velocities = jointCommands
self.arm_joint_vel_pub.publish(desiredVelocities)
rospy.wait_for_service('/tray_controller/init')
print "found init"
initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
resp = initService()
velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv1 = JointValue()
vv1.timeStamp = rospy.Time.now()
vv1.joint_uri = "tray_1_joint"
vv2 = copy.deepcopy(vv1)
vv2.joint_uri = "tray_2_joint"
vv3 = copy.deepcopy(vv1)
vv3.joint_uri = "tray_3_joint"
v.velocities = [vv1,vv2,vv3]
v.velocities[0].value = 0.0
while not rospy.is_shutdown():
v.velocities[0].value = 0.05
v.velocities[1].value = 0.05
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = 0
v.velocities[1].value = 0
velPublisher.publish(v)
rospy.sleep(1.0)
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.5
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.5
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 3.0
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
rospy.init_node("ipa_canopen_test")
rospy.wait_for_service('/mockarm_controller/init')
initService = rospy.ServiceProxy('/mockarm_controller/init', Trigger)
resp = initService()
print resp
velPublisher = rospy.Publisher('/mockarm_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv = JointValue()
vv.timeStamp = rospy.Time.now()
vv.joint_uri = "mockarm_1_joint"
v.velocities = [vv]
while not rospy.is_shutdown():
v.velocities[0].value = 0.2
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = 0
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = - 0.2
velPublisher.publish(v)
rospy.wait_for_service('/tray_controller/init')
print "found init"
initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
resp = initService()
velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv1 = JointValue()
vv1.timeStamp = rospy.Time.now()
vv1.joint_uri = "tray_1_joint"
vv2 = copy.deepcopy(vv1)
vv2.joint_uri = "tray_2_joint"
vv3 = copy.deepcopy(vv1)
vv3.joint_uri = "tray_3_joint"
v.velocities = [vv1, vv2, vv3]
v.velocities[0].value = 0.0
while not rospy.is_shutdown():
v.velocities[0].value = 0.05
v.velocities[1].value = 0.05
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = 0
v.velocities[1].value = 0
velPublisher.publish(v)
rospy.sleep(1.0)
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.5
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 3.0
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
gripper_pos_msg.positions = [gripper_pos]
gripper_pos_pub.publish(gripper_pos_msg)
rospy.sleep(5.0)
# send joint velocities
gripper_vel_msg = JointVelocities()
gripper_vel = JointValue()
gripper_vel.timeStamp = rospy.Time.now()
gripper_vel.joint_uri = 'left_arm_top_finger_joint'
gripper_vel.unit = 'm/s'
gripper_vel_msg.velocities = [gripper_vel]
# while loop
t_start = rospy.Time.now()
A = 8e-3
f = 0.5
loop_rate = rospy.Rate(500)
while not rospy.is_shutdown():
gripper_vel_msg.velocities[0].value = (A)*(2*np.pi*f)*np.sin(2*np.pi*f*(rospy.Time.now()-t_start).to_sec())
gripper_vel_pub.publish(gripper_vel_msg)
loop_rate.sleep()
