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()