def main(self):
# rospy.init_node('putitback')
arm_pub = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
# arm_ik_control.go_to_xyz(0.1, -0.2, 0.3, arm_pub)
initial_position.main()
gripper_open.main()
msg = JointPositions()
rospy.sleep(5)
print "moving joint 1" #move joint 3 and joint 2
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].unit = "rad"
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.04
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].value = 2.8
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.01005
arm_pub.publish(msg)
rospy.sleep(3)
print "moving joint 2"
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].value = 1.7
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].value = -0.6
arm_pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].value = 1.4
arm_pub.publish(msg)
gripper_close.main()
rospy.sleep(2)
initial_position.main()
arm_ik_control.go_to_xyz_rev(-0.069, -0.447, 0.12, arm_pub)
def drop_off(self, object_name):
# self.object_name = object_name
# beacon1 = xyz_vicon.xyz_vicon(object_name)
rospy.sleep(1)
print "now working"
gripper_open.main()
gripper_close.main()
rospy.sleep(3)
arm_ik_control.go_to_xyz_rev(0.2, -0.4, 0.1, self.arm_pub)
gripper_open.main()
gripper_close.main()
rospy.sleep(3)
put_down.main()
initial_position.main()
def main():
pub = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
rospy.init_node('arm_test')
'''initial positions'''
msg = JointPositions()
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -0.016
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0222
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0111
pub.publish(msg)
# time.sleep(1)
# msg.positions[0].joint_uri = "arm_joint_1"
# msg.positions[0].value = 1.3
# pub.publish(msg)
#
# msg.positions[0].joint_uri = "arm_joint_2"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 1.3
# pub.publish(msg)
#
# msg.positions[0].joint_uri = "arm_joint_3"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = -1.2
# pub.publish(msg)
#
# time.sleep(2)
#
# msg.positions[0].joint_uri = "arm_joint_4"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 2.8
# pub.publish(msg)
# gripper_open.main()
#
# time.sleep(2)
#
#
# msg.positions[0].joint_uri = "arm_joint_2"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 2
# pub.publish(msg)
#
# gripper_close.main()
# time.sleep(2)
#
# msg.positions[0].joint_uri = "arm_joint_2"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 0.5
# pub.publish(msg)
#
# time.sleep(2)
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.075
pub.publish(msg)
time.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 1.3
pub.publish(msg)
time.sleep(1)
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 2.85
pub.publish(msg)
gripper_open.main()
time.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.5
pub.publish(msg)
rospy.spin()
def drop_off(self, object_name):
# self.object_name = object_name
# beacon1 = xyz_vicon.xyz_vicon(object_name)
rospy.sleep(1)
print "now working"
gripper_open.main()
gripper_close.main()
rospy.sleep(3)
arm_ik_control.go_to_xyz_rev(0.2, -0.4, 0.1, self.arm_pub)
gripper_open.main()
gripper_close.main()
rospy.sleep(3)
put_down.main()
initial_position.main()
if __name__ == '__main__':
name_of_object = 'cup2'
arm = arm()
gripper_open.main()
rospy.sleep(1)
arm.go_to_position(name_of_object)
# arm.drop_off(name_of_object)
# rospy.sleep(3)
# putdown.main()
def main(self):
pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size = 10)
# rospy.init_node('arm_test')
'''initial positions'''
msg = JointPositions()
print "going to initial position"
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -0.016
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0222
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.111
pub.publish(msg)
rospy.sleep(1)
print "time to move"
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.011
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 1.3
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -1.2
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 2.8
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 2.5
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -1
pub.publish(msg)
gripper_open.main()