def __init__(self, pose=[0, 0, 0, 0, 0, 0, 1], description='Target', server_name='target_marker', frame_id='exotica/world_frame', marker_shape=Marker.ARROW, marker_size=[0.2, 0.05, 0.05], controls=[]): self.position = kdl.Frame() self.position_exo = exo.KDLFrame() self.server = InteractiveMarkerServer(server_name) self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = frame_id self.int_marker.name = server_name self.int_marker.description = description self.int_marker.pose = list_to_pose(pose) self.position_exo = exo.KDLFrame(pose) self.position = kdl.Frame( kdl.Rotation.Quaternion(self.int_marker.pose.orientation.x, self.int_marker.pose.orientation.y, self.int_marker.pose.orientation.z, self.int_marker.pose.orientation.w), kdl.Vector(self.int_marker.pose.position.x, self.int_marker.pose.position.y, self.int_marker.pose.position.z)) # create a grey box marker visual_marker = Marker() visual_marker.type = marker_shape visual_marker.scale.x = marker_size[0] visual_marker.scale.y = marker_size[1] visual_marker.scale.z = marker_size[2] visual_marker.color.r = 0.0 visual_marker.color.g = 0.5 visual_marker.color.b = 0.5 visual_marker.color.a = 1.0 # create a non-interactive control which contains the box visual_control = InteractiveMarkerControl() visual_control.always_visible = True visual_control.markers.append(visual_marker) # add the control to the interactive marker self.int_marker.controls.append(visual_control) self.addControl([1, 0, 0], InteractiveMarkerControl.ROTATE_AXIS) self.addControl([0, 1, 0], InteractiveMarkerControl.ROTATE_AXIS) self.addControl([0, 0, 1], InteractiveMarkerControl.ROTATE_AXIS) self.addControl([1, 0, 0], InteractiveMarkerControl.MOVE_AXIS) self.addControl([0, 1, 0], InteractiveMarkerControl.MOVE_AXIS) self.addControl([0, 0, 1], InteractiveMarkerControl.MOVE_AXIS) for control in controls: self.int_marker.controls.append(control) self.server.insert(self.int_marker, self.process_feedback) self.server.applyChanges()
def transform_base_traj(base_pose, tf_base): '''takes input of base_pose which is an array [x,y,rot] and a KDLFrame. returns [x,y,z,r,p,yaw] ''' # frame = exo.KDLFrame(np.concatenate((base_pose[0:2],[0,0,0],base_pose[2]),axis = 0)) frame = exo.KDLFrame(np.concatenate((base_pose[0:2],[0,0,0],[base_pose[2]]))) rel_goal = tf_base * frame return rel_goal.get_translation_and_rpy()
def addControl(self, direction, control_type): control = InteractiveMarkerControl() quat = exo.KDLFrame([0, 0, 0] + direction + [1]).get_quaternion() control.orientation.x = quat[0] control.orientation.y = quat[1] control.orientation.z = quat[2] control.orientation.w = quat[3] control.interaction_mode = control_type self.int_marker.controls.append(control)
def handleAttaching(i, x, scene): # Update state scene.Update(x) if i == 0: # Reset object pose scene.attachObjectLocal('Item', '', exo.KDLFrame([0.6, -0.3, 0.5, 0, 0, 0, 1])) if i == 1: # Attach to end effector scene.attachObject('Item', 'lwr_arm_6_link') if i == 2: # Detach scene.detachObject('Item')
def process_feedback(self, feedback): self.position = kdl.Frame( kdl.Rotation.Quaternion(feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w), kdl.Vector(feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z)) self.position_exo = exo.KDLFrame([ feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w ])
def send_trajectory(received_traj,grasp_times, dt): # set up hsr python interface robot = hsrb_interface.Robot() whole_body = robot.get('whole_body') hsrb_gripper = robot.get('gripper') omni_base = robot.get('omni_base') try: open_gripper(hsrb_gripper) whole_body.move_to_go() except: tts.say('Fail to initialize.') rospy.logerr('fail to init') sys.exit() print('going to start_location') # go to starting location. listener = tf.TransformListener() listener.waitForTransform('/map','/my_start_pos', rospy.Time(0),rospy.Duration(3.0)) (start_pos, start_quat) = listener.lookupTransform('/map','/my_start_pos', rospy.Time(0)) start_state = exo.KDLFrame(start_pos + start_quat) x,y,z,roll,pitch,yaw = start_state.get_translation_and_rpy() print(yaw) # go to starting position omni_base.go_abs(x,y,yaw,1000.0) whole_body.move_to_go() # exit() '''takes input of trajectory from hsr_meeting_table_aico, [grasp_start, grasp_duration] and time step of trajectory in seconds. ''' #default dt = 0.1. set to >0.1 for testing / velocity limits exceeded dt = 0.15 # dt = 0.10 print("received") print(np.shape(received_traj)) # midpoint = grasp_times[1]/2 grasp_t = grasp_times[0]# + midpoint # workaround # cli_arm, cli_base, cli_grip_follow_traj, cli_grip_force = setup() cli_arm, cli_base = setup() # set current hsr base position to zero so I can run trajectories multiple times by resetting # gazebo with gazebo_reset. base_posestamped = rospy.wait_for_message('/global_pose', PoseStamped) base_pose = base_posestamped.pose x = base_pose.position.x y = base_pose.position.y z = base_pose.position.z qx= base_pose.orientation.x qy= base_pose.orientation.y qz= base_pose.orientation.z qw= base_pose.orientation.w tf_base = exo.KDLFrame([x,y,z,qx,qy,qz,qw]) time_list = np.arange(0.0,dt*len(received_traj),dt) # rearrange traj arm_list = received_traj[:,3:8] base_list = received_traj[:,0:3] arm_list = np.c_[arm_list,time_list] # move relative to hsrb_base_link instead of map base_list = np.array([transform_base_traj(base_list[i], tf_base) for i in range(len(base_list))]) #index of x,y,yaw col_index = [0,1,5] base_list = base_list[:, col_index] base_list = np.c_[base_list, time_list] # #remove the zero-th time step because it has an undefined velocity base_list = base_list[1::] arm_list = arm_list[1::] # get user input to run traj in gazebo print("you can run ./log_vel.py now.") print("press enter to continue") raw_input() print("loading arm goal") arm.load_arm_goal(arm_list,cli_arm,dt) print("loading base goal") base.load_base_goal(base_list,cli_base,dt) # gripper controller doesnt work. not listed in lise_controllers. See setup(). on around line 30 # thus jank stuff with rospy sleep to get the gripper to close at the right time print("loading gripper goal") # gripper.load_gripper_goal(gripper_list,cli_grip_follow_traj) #open gripper # workaround # gripper.load_gripper_goal(np.array([[0.8,0.5]]),cli_grip_follow_traj) open_gripper(hsrb_gripper) print("sleeping") rospy.sleep(grasp_t*dt/0.1) print("closing") # close_gripper(cli_grip_force) close_gripper(hsrb_gripper) while not rospy.is_shutdown(): rospy.spin() print('All Done :)') print('shutting down rospy') exit()
#!/usr/bin/env python import pyexotica as exo solver = exo.Setup.loadSolver( '{exotica_examples}/resources/configs/ik_solver_demo.xml') scene = solver.getProblem().getScene() # Update the scene before calling FK or Jacobian! scene.Update([0] * 7) print("FK for the end effector:") print(scene.fk('lwr_arm_7_link')) print("Jacobian for the end effector:") print(scene.jacobian('lwr_arm_7_link')) print("FK for a relative frame:") scene.Update([0.5] * 7) print(scene.fk('lwr_arm_7_link', 'lwr_arm_3_link')) print("FK for a relative frame with custom offsets:") print( scene.fk('lwr_arm_7_link', exo.KDLFrame([0, 0, 0.2, 0, 0, 0, 1]), 'lwr_arm_3_link', exo.KDLFrame())) # FK and Jacobian have the same overloads
listener = tf.TransformListener() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: # get tranforms of bottle and table and tag_1 (edge of table) relative to map (table_pos, table_quat) = listener.lookupTransform('/map', '/table', rospy.Time(0)) (bottle_pos, bottle_quat) = listener.lookupTransform('/map', '/hsr_soda', rospy.Time(0)) (table_edge_pos, table_edge_quat) = listener.lookupTransform( '/map', '/tag_1', rospy.Time(0)) table = exo.KDLFrame(table_pos + table_quat) bottle = exo.KDLFrame(bottle_pos + bottle_quat) tag_1 = exo.KDLFrame(table_edge_pos + table_edge_quat) # TODO(2021-12-20): Since the AprilTag recognition is not stable in orientation with the # simulator on Noetic, overwrite the transform to 0,0,0 rpy # THIS IS A HACK and should not be used on hardware/fixed later. table = exo.KDLFrame(table_pos + [pi / 2, 0, 0]) bottle = exo.KDLFrame(bottle_pos + [0, 0, 0]) tag_1 = exo.KDLFrame(table_edge_pos + [0, 0, 0]) # table relative to bottle. Can find how to rotate bottle tf to match table # how to rotate bottle to get to table tf bottle_table = bottle.inverse() * table # rotate the bottle tf so that it matches the orientation of the table
def send_trajectory(received_traj,grasp_times, dt): robot = hsrb_interface.Robot() whole_body = robot.get('whole_body') hsrb_gripper = robot.get('gripper') omni_base = robot.get('omni_base') try: open_gripper(hsrb_gripper) whole_body.move_to_go() except: tts.say('Fail to initialize.') rospy.logerr('fail to init') sys.exit() # print('going to start_location') # listener = tf.TransformListener() # listener.waitForTransform('/map','/my_start_pos', rospy.Time(0),rospy.Duration(3.0)) # (start_pos, start_quat) = listener.lookupTransform('/map','/my_start_pos', rospy.Time(0)) # start_state = exo.KDLFrame(start_pos + start_quat) # x,y,z,roll,pitch,yaw = start_state.get_translation_and_rpy() # print(yaw) # omni_base.go_abs(x,y,yaw,1000.0) # whole_body.move_to_go() whole_body.move_to_neutral() # exit() '''takes input of trajectory from hsr_meeting_table_aico, [grasp_start, grasp_duration] and time step of trajectory in seconds. ''' #default dt = 0.1. set to >0.1 for testing / velocity limits exceeded dt = 0.2 # dt = 0.10 print("received") print(np.shape(received_traj)) # midpoint = grasp_times[1]/2 grasp_t = grasp_times# + midpoint # workaround # cli_arm, cli_base, cli_grip_follow_traj, cli_grip_force = setup() cli_arm, cli_base = setup() base_posestamped = rospy.wait_for_message('/global_pose', PoseStamped) base_pose = base_posestamped.pose x = base_pose.position.x y = base_pose.position.y z = base_pose.position.z qx= base_pose.orientation.x qy= base_pose.orientation.y qz= base_pose.orientation.z qw= base_pose.orientation.w tf_base = exo.KDLFrame([x,y,z,qx,qy,qz,qw]) time_list = np.arange(0.0,dt*len(received_traj),dt) arm_list = received_traj[:,4:9] base_list = received_traj[:,1:4] arm_list = np.c_[arm_list,time_list] base_list= base_list - base_list[0] # base_list = base_list - base_list[0] to_file(base_list, "base_list.txt") # move relative to hsrb_base_link instead of map # base_list = np.array([transform_base_traj(base_list[i], tf_base) for i in range(len(base_list))]) # to_file(base_list, "base_list_t.txt") #index of x,y,yaw col_index = [0,1,5] # base_list = base_list[:, col_index] base_list = np.c_[base_list, time_list] # #remove the zero-th time step because it has an undefined velocity base_list = base_list[1::] arm_list = arm_list[1::] # exit() print("you can run ./log_vel.py now.") print("press enter to continue") input() print("loading arm goal") arm.load_arm_goal(arm_list,cli_arm,dt) print("loading base goal") base.load_base_goal(base_list,cli_base,dt) print("loading gripper goal") # gripper.load_gripper_goal(gripper_list,cli_grip_follow_traj) #open gripper # workaround # gripper.load_gripper_goal(np.array([[0.8,0.5]]),cli_grip_follow_traj) open_gripper(hsrb_gripper) print("sleeping") # gripper state = open gripper_state = 0 # control gripper with this weirdness since action server for gripper doesnt work if len(grasp_t) > 1: gripper_action_times=np.diff(grasp_t) for i in range(len(grasp_t)): rospy.sleep(gripper_action_times*dt/0.1) if gripper_state == 0: close_gripper(hsrb_gripper) else: open_gripper(hsrb_gripper) print("closing") # close_gripper(cli_grip_force) while not rospy.is_shutdown(): rospy.spin() print('All Done :)') print('shutting down rospy') exit()
'{exotica_examples}/tests/resources/PrimitiveSphere_vs_PrimitiveSphere_Distance.urdf', '{exotica_examples}/tests/resources/Mesh_vs_Mesh_Distance.urdf' ] for urdf in urdfs_to_test: print("Testing", urdf) initializer = getProblemInitializer(collisionScene, urdf) prob = exo.Setup.createProblem(initializer) prob.update(np.zeros(prob.N, )) scene = prob.getScene() cs = scene.getCollisionScene() # Should collide at -2 p = cs.continuousCollisionCheck("A_collision_0", exo.KDLFrame([-3., 0.0, 0.0]), exo.KDLFrame([-1.0, 0.0, 0.0]), "B_collision_0", exo.KDLFrame([0, 0, 0]), exo.KDLFrame([0, 0, 0])) assert (p.InCollision == True) assert ((p.TimeOfContact - 0.5) < 0.1) assert (np.isclose(p.ContactTransform1.getTranslation(), np.array([-2, 0, 0]), atol=0.15).all()) assert (np.isclose(p.ContactTransform2.getTranslation(), np.array([0, 0, 0])).all()) print(p) print('>>SUCCESS<<') exit(0)
def start_aico(): do_plot = True traj_version = -1 use_screenshot = False # ffmpeg -r 50 -f image2 -s 1920x1080 -i ./hsr_driveby_visualisation_%03d.png -vcodec libx264 -pix_fmt yuv420p ./output.mp4 screenshot = lambda *args: None if use_screenshot: from jsk_rviz_plugins.srv import Screenshot, ScreenshotRequest, ScreenshotResponse rospy.wait_for_service('/rviz/screenshot') screenshot = rospy.ServiceProxy('/rviz/screenshot', Screenshot) exo.Setup.init_ros() # config_name = '{hsr_driveby_full}/resources/hsr_meeting_room_table_aico_new.xml' config_name = '{hsr_driveby_full}/resources/hsr_meeting_room_table_aico.xml' solver = exo.Setup.load_solver(config_name) problem = solver.get_problem() scene = problem.get_scene() kt = scene.get_kinematic_tree() joint_limits = problem.get_scene().get_kinematic_tree().get_joint_limits() # Set target for soda can scene.attach_object("SodaCan", "TargetObject") #scene.attach_object_local("TargetObject", "Table", exo.KDLFrame([0.2,0.30,0.06+0.04])) #added offset to the y coordinate since planning and simulation grasp objects have different geometry. # bottle on right hand side # <!-- made changes here ** --> scene.attach_object_local("TargetObject", "Table", exo.KDLFrame([0.2,0.30,0.06]))#+0.04])) # bottle on left hand side scene.attach_object_local("TargetObject", "Table", exo.KDLFrame([0.2,0.30,0.06]))#+0.04])) # Move robot to start state x_start = problem.start_state x_start[0] = 0 x_start[1] = 0 x_start[2]= 0 # x_start[2] = -np.pi/2. problem.start_state = x_start scene.set_model_state(problem.start_state) #scene.set_model_state_map({'hand_motor_joint': 0.7, 'hand_l_spring_proximal_joint':0.9, 'hand_l_distal_joint': -0.6, 'hand_r_spring_proximal_joint':0.9, 'hand_r_distal_joint': -0.6, 'wrist_roll_joint': -1.}) #set to move to go, assuming robot is already moving # <!-- made changes here ** --> # scene.set_model_state_map({'hand_motor_joint': 0.7, 'hand_l_spring_proximal_joint':0.9, 'hand_l_distal_joint': -0.6, 'hand_r_spring_proximal_joint':0.9, 'hand_r_distal_joint': -0.6, 'wrist_roll_joint': 0, 'wrist_flex_joint': -np.pi/2, 'arm_roll_joint': -np.pi/2}) scene.set_model_state_map({'hand_motor_joint': 0.7, 'hand_l_spring_proximal_joint':0.9, 'hand_l_distal_joint': -0.6, 'hand_r_spring_proximal_joint':0.9, 'hand_r_distal_joint': -0.6, 'wrist_roll_joint': 0, 'wrist_flex_joint': -np.pi/2, 'arm_roll_joint': 0}) problem.start_state = scene.get_model_state() q_start = problem.apply_start_state(True) q_start = np.clip(q_start, joint_limits[:,0], joint_limits[:,1]) problem.update(q_start, 0) problem.start_state = scene.get_model_state() q_start = problem.apply_start_state(True) if np.any(q_start < joint_limits[:,0]) or np.any(q_start > joint_limits[:,1]): raise RuntimeError("Start state exceeds joint limits!") mug_location = scene.fk('SodaCan', exo.KDLFrame(), '', exo.KDLFrame()).get_translation_and_rpy() # t_grasp_begin = 3.5 #4.2 t_grasp_begin = 4.5 t_grasp_duration = 0.5 T_grasp_begin = int(t_grasp_begin / problem.tau) T_grasp_end = int((t_grasp_begin + t_grasp_duration) / problem.tau) # The target position needs to be reached during the grasping period problem.set_rho('Position', 0, 0) for t in range(T_grasp_begin, T_grasp_end): problem.set_rho('Position', 1e4, t) problem.set_goal('Position', mug_location[:3], t) # The HSR has a poor reachability, so we deactivate the base tracking here # problem.set_rho('BasePosition', 0, t) # Height above the table before and after grasp problem.set_rho('LiftOffTable', 1e2, T_grasp_begin - 20) # problem.set_rho('LiftOffTable', 1e3, T_grasp_end + 5) # problem.set_rho('LiftOffTable', 1e4, T_grasp_end + 10) problem.set_rho('LiftOffTable', 1e2, T_grasp_end + 20) # The axis needs to be fixed from the beginning of the grasp to the end of the motion for t in range(T_grasp_begin, problem.T): #problem.set_rho('AxisAlignment', 1e4, t) problem.set_rho('AxisAlignment', 1e2, t) problem.set_rho('BaseOrientation', 1e2, -1) # Initial trajectory = zero motion zero_motion = np.zeros((problem.T,problem.N)) for t in range(problem.T): zero_motion[t,:] = q_start problem.initial_trajectory = zero_motion solution = solver.solve() print("Solved in", solver.get_planning_time(), "final cost", problem.get_cost_evolution()[1][-1]) # ''' # Show convergence plot fig = plt.figure(1) plt.plot(problem.get_cost_evolution()[0], problem.get_cost_evolution()[1]) plt.yscale('log') plt.ylabel('Cost') plt.xlabel('Time (s)') plt.xlim(0,np.max(problem.get_cost_evolution()[0])) plt.title('Convergence') # Show cost breakdown fig = plt.figure(2) # ''' if do_plot: costs = {} ct = 1.0 / problem.tau / problem.T for t in range(problem.T): problem.update(solution[t,:],t) for cost_task in problem.cost.indexing: task = problem.cost.tasks[cost_task.id] task_name = task.name task_id = task.id costs[task_name] = np.zeros((problem.T,)) # print(task_id, task_name, task, cost_task.start, cost_task.length, cost_task.startJ, cost_task.lengthJ) for t in range(problem.T): ydiff = problem.cost.ydiff[t][cost_task.startJ:cost_task.startJ+cost_task.lengthJ] rho = problem.cost.S[t][cost_task.startJ:cost_task.startJ+cost_task.lengthJ,cost_task.startJ:cost_task.startJ+cost_task.lengthJ] cost = np.dot(np.dot(ydiff, rho), ydiff) costs[task_name][t] = ct * cost # ''' if do_plot: costs['Task'] = np.zeros((problem.T,)) costs['Transition'] = np.zeros((problem.T,)) for t in range(problem.T): costs['Task'][t] = problem.get_scalar_task_cost(t) costs['Transition'][t] = problem.get_scalar_transition_cost(t) for cost in costs: plt.plot(costs[cost], label=cost) plt.legend() plt.xlim(0,problem.T) plt.title('Cost breakdown across trajectory per task') plt.show() plot(solution, labels=scene.get_controlled_joint_names()) print(mug_location) return solution publish_trajectory(solution, problem.T*problem.tau, problem)