def __init__(self, limb, reconfig_server, rate=100.0, mode='joint_impedance', sim = False, interpolation='minjerk'): self._mode = mode self._dyn = reconfig_server self._ns = limb + '/joint_trajectory_controller' self._fjt_ns = self._ns + '/follow_joint_trajectory2' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._limb = BorisRobot() self._name = limb self._interpolation = interpolation self._server.start() self._alive = True # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() self._limb.set_control_mode(mode=self._mode) if sim: rospy.loginfo("Setting gains for simulated robot") self._limb.set_joint_impedance(np.array([2000,2000,2000,2000,2000,2000,2000]),np.array([100,100,100,100,100,100,100])) # Create our spline coefficients self._coeff = [None] * len(self._limb.joint_names(self._name)) # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(self._control_rate)
def __init__(self): self._moveit_wrapper = MoveitWrapper() self._moveit_wrapper.init_moveit_commander() rospy.init_node('grasp_player') self._moveit_wrapper.setup() self._kin = boris_kinematics(root_link="left_arm_base_link") self._boris = BorisRobot(moveit_wrapper=self._moveit_wrapper) self._scene = self._moveit_wrapper.scene() self._planning_frame = self._moveit_wrapper.robot().get_planning_frame( ) self._tf_buffer = tf2_ros.Buffer() self._listener = tf2_ros.TransformListener(self._tf_buffer) self._br = tf.TransformBroadcaster() self._scene.remove_world_object("table") self.add_table() self._solution = None self._grasp_waypoints = [] self._pre_grasp_plan = None self._pre_grasp_plan2 = None self._grasp_arm_joint_path = None self._grasp_hand_joint_path = None self._post_grasp_plan = None self._grasp_service_client = GraspServiceClient() self._boris.set_control_mode(mode="joint_impedance", limb_name="left_arm") self._arm_traj_generator = MinJerkTrajHelper() self._hand_traj_generator = MinJerkTrajHelper() self._pre_grasp_traj_generator = MinJerkTrajHelper() self._post_grasp_traj_generator = MinJerkTrajHelper() self._scan_waypoints = np.load( "/home/earruda/Projects/boris_ws/src/boris-robot/boris_tools/scripts/scan_waypoints2.npy" ) self._kbhit = KBHit()
class GravityCompensation(object): def __init__(self): self._robot = BorisRobot() # atexit.register(self._cic.stop) def run(self): from getch import getch finish = False print("Press enter to record to next. Add/Remove/Calibrate/Help? (a/r/c/h)") rate = rospy.Rate(10) self._robot.wait_enabled() self._robot.set_gravity_compesantion_mode() while not finish: print self._robot.angles('left_arm') rate.sleep() finish = rospy.is_shutdown()
def main(): moveit_wrapper = MoveitWrapper() moveit_wrapper.init_moveit_commander() rospy.init_node('aml_trajectory_player') moveit_wrapper.setup() boris = BorisRobot(moveit_wrapper=moveit_wrapper) # trajectory = [-2.02830171585, 1.04696202278, 1.55436050892, -1.44723391533, -0.815707325935, 0.387918055058, -2.68925023079] joint_names = rospy.get_param("left_arm/joints") hand_joint_names = rospy.get_param("left_hand/joints") trajectories = [ 'suitcase_trajectory01.csv', 'cylinder_traj.csv', 'cylinder_slide_trajectory.csv', 'test_traj.csv', 'box_traj.csv', 'ibuprofen_trajectory.csv' ] trajectory, _ = parse_trajectory_file("contact_trajectories/" + trajectories[1]) traj_msg = make_ros_trajectory_msg(trajectory, joint_names, index_map=(1, 8)) traj_hand_msg = make_ros_trajectory_msg(trajectory, hand_joint_names[:1], index_map=(8, 9)) rate = rospy.Rate(30) while not rospy.is_shutdown(): c = raw_input("send: ") if c == "y": boris.follow_trajectory("left_arm", traj_msg, first_waypoint_moveit=True) boris.follow_trajectory("left_hand", traj_hand_msg, first_waypoint_moveit=False) if c == "h": boris.stop_trajectories() rate.sleep()
def main(): rospy.init_node('test_cartesian_trajectory') kin = boris_kinematics(root_link="left_arm_base_link") boris = BorisRobot() jic = JointImpedanceCommander() jic.stop() rate = rospy.Rate(30) jic.activate() joint_names = boris.joint_names() print joint_names[:7] neg_keys = ['a', 's', 'd', 'f', 'g', 'h', 'j'] pos_keys = ['q', 'w', 'e', 'r', 't', 'y', 'u'] key_map_neg = dict(zip(neg_keys, range(7))) key_map_pos = dict(zip(pos_keys, range(7))) delta_angle = 0.05 jic.send_damping([25, 25, 25, 25, 25, 5, 2.0]) #[0.1,0.1,0.1,0.1,0.1,0.1,0.1] jic.send_stiffness([200, 200, 200, 100, 100, 50, 25]) #[800,800,800,800,300,300,500] while not rospy.is_shutdown(): joint_angles = boris.joint_angles() angles = np.array([joint_angles[name] for name in joint_names[:7]]) current_angles = np.array( [joint_angles[name] for name in joint_names[:7]]) key = getch(timeout=0.5) #raw_input("key: ") if key in pos_keys: idx = key_map_pos[key] angles[idx] += delta_angle cmd = jic.compute_command(angles) print "Current: ", current_angles print "Command: ", cmd.data jic.send_command(cmd) elif key in neg_keys: idx = key_map_neg[key] angles[idx] -= delta_angle cmd = jic.compute_command(angles) print "Current: ", current_angles print "Command: ", cmd.data jic.send_command(cmd) elif key in ['\x1b', '\x03']: rospy.signal_shutdown("Finished.") break rate.sleep()
def main(): moveit_wrapper = MoveitWrapper() moveit_wrapper.init_moveit_commander() rospy.init_node('test_cartesian_trajectory') moveit_wrapper.setup() kin = boris_kinematics(root_link="left_arm_base_link") boris = BorisRobot(moveit_wrapper=moveit_wrapper) # trajectory = [-2.02830171585, 1.04696202278, 1.55436050892, -1.44723391533, -0.815707325935, 0.387918055058, -2.68925023079] joint_names = rospy.get_param("left_arm/joints") hand_joint_names = rospy.get_param("left_hand/joints") trajectories = [ 'suitcase_trajectory01.csv', 'cylinder_traj.csv', 'cylinder_slide_trajectory.csv', 'test_traj.csv', 'box_traj.csv', 'ibuprofen_trajectory.csv' ] trajectory, _ = parse_trajectory_file('contact_trajectories/' + trajectories[1]) traj_msg = make_ros_trajectory_msg(trajectory, joint_names, index_map=(1, 8)) traj_hand_msg = make_ros_trajectory_msg(trajectory, hand_joint_names[:1], index_map=(8, 9)) cartesian_traj = make_cartesian_trajectory(trajectory, index_map=(1, 8), fk_func=kin.forward_kinematics) traj_helper = MinJerkTrajHelper() traj_helper_hand = MinJerkTrajHelper() traj_helper_hand.set_waypoints(traj_hand_msg) traj_helper.set_waypoints(traj_msg) print traj_msg.points[0].time_from_start.to_sec() time_steps = np.linspace(0, 1, 1000) boris.exit_control_mode() boris.set_control_mode(mode="joint_impedance", limb_name="left_arm") boris.goto_with_moveit("left_arm", traj_msg.points[0].positions) i = 0 loop_rate = rospy.Rate(30) while not rospy.is_shutdown(): # key = raw_input("key: ") # if key=='n' and i < len(time_steps)-1: # i += 1 # elif key=='b' and i > 0: # i -= 1 joint_goal = traj_helper.get_point_t(time_steps[i]) hand_goal = traj_helper_hand.get_point_t(time_steps[i]) print "Goal: ", hand_goal.time_from_start.to_sec( ), " i=", i, " pos: ", hand_goal.positions cmd = boris.cmd_joint_angles(angles=joint_goal.positions, execute=True) boris.goto_joint_angles('left_hand', hand_goal.positions) loop_rate.sleep() i += 1 if i >= len(time_steps) - 1: i = len(time_steps) - 1 # for i in np.linspace(0,1,10): # print traj_helper.get_point_t(i).time_from_start.to_sec() # boris.exit_control_mode() # # plan_traj = plan.joint_trajectory # t0 = rospy.Time.now() # i = 0 # n_wpts = len(plan_traj.points) # total_time = plan_traj.points[-1].time_from_start.to_sec() # frequency = n_wpts/total_time # print "Frequency: ", frequency # rate = rospy.Rate(frequency) # # boris.set_control_mode(mode="joint_impedance", limb_name="left_arm") # while not rospy.is_shutdown(): # pass # cmd = None # for joint_goal in (plan_traj.points): # cmd = boris.cmd_joint_angles(angles=joint_goal.positions,execute=True) # print "Time: ", (rospy.Time.now()-t0).to_sec(), " Expect: ", joint_goal.time_from_start.to_sec() # print "Cmd: ", cmd.data # i+=1 # # c = raw_input("next: ") # if rospy.is_shutdown(): # break # rate.sleep() # tf = rospy.Time.now() # print "Final Time: ", (tf-t0).to_sec(), " Expect: ", trajectory[-1][0] rospy.sleep(3.0)
#!/usr/bin/env python import argparse, sys from copy import copy import rospy from boris_tools.recorder import JointRecorder import numpy as np from boris_tools.boris_robot import BorisRobot recording = False pressed = False robot = BorisRobot() def main(): print("Initialising node... ") rospy.init_node("joint_trajectory_recorder_node") rate = rospy.Rate(20) # print traj.result() recorder = JointRecorder(sys.argv[1], 30.0, robot) rospy.on_shutdown(recorder.stop) print("Recording. Press Ctrl-C to stop.") recorder.record() # while not rospy.is_shutdown(): # # print("hand angle: %lf"%(robot.joint_angle('left_hand_synergy_joint')))
def main(): moveit_wrapper = MoveitWrapper() moveit_wrapper.init_moveit_commander() rospy.init_node('test_cartesian_trajectory') moveit_wrapper.setup() kin = boris_kinematics(root_link="left_arm_base_link") boris = BorisRobot(moveit_wrapper=moveit_wrapper) # trajectory = [-2.02830171585, 1.04696202278, 1.55436050892, -1.44723391533, -0.815707325935, 0.387918055058, -2.68925023079] joint_names = rospy.get_param("left_arm/joints") hand_joint_names = rospy.get_param("left_hand/joints") trajectories = [ 'suitcase_trajectory01.csv', 'cylinder_traj.csv', 'cylinder_slide_trajectory.csv', 'test_traj.csv', 'box_traj.csv', 'ibuprofen_trajectory.csv' ] trajectory, _ = parse_trajectory_file('contact_trajectories/' + trajectories[1]) traj_msg = make_ros_trajectory_msg(trajectory, joint_names, index_map=(1, 8)) traj_hand_msg = make_ros_trajectory_msg(trajectory, hand_joint_names[:1], index_map=(8, 9)) cartesian_traj = make_cartesian_trajectory(trajectory, index_map=(1, 8), fk_func=kin.forward_kinematics) boris.exit_control_mode() boris.goto_with_moveit('left_arm', traj_msg.points[0].positions) boris.stop_trajectory('left_arm') boris.set_control_mode(mode="cartesian_impedance", limb_name="left_arm") print kin._base_link print kin._tip_link # print cartesian_traj t0 = rospy.Time.now() i = 0 n_wpts = len(trajectory) total_time = trajectory[-1][0] frequency = n_wpts / total_time print "Frequency: ", frequency rate = rospy.Rate(frequency) boris.follow_trajectory("left_hand", traj_hand_msg, first_waypoint_moveit=False) cmd = None for pose in cartesian_traj: boris.cmd_cartesian_ee((pose[:3], pose[3:])) print "Time: ", (rospy.Time.now() - t0).to_sec(), " Expect: ", trajectory[i][0] i += 1 # c = raw_input("next: ") if rospy.is_shutdown(): break rate.sleep() tf = rospy.Time.now() print "Final Time: ", (tf - t0).to_sec(), " Expect: ", trajectory[-1][0] # Hold for 3s with higher stiffness before switching to position control (get closer to the last goal) # hold_time0 = rospy.Time.now().to_sec() # pose = cic.get_current_pose() # hold_rate = rospy.Rate(200) # while (rospy.Time.now().to_sec() - hold_time0) < 10.0: # cmd = cic.compute_command(pose) # cmd.k_FRI.x = cmd.k_FRI.y = cmd.k_FRI.z = 500 # cmd.k_FRI.rx = cmd.k_FRI.ry = cmd.k_FRI.rz = 50 # cic.send_command(cmd) # hold_rate.sleep() # cic.stop() # rospy.sleep(1.0) # boris.goto_with_moveit('left_arm', traj_msg.points[0].positions) # boris.stop_trajectory('left_arm') rospy.sleep(3.0)
def main(): moveit_wrapper = MoveitWrapper() moveit_wrapper.init_moveit_commander() rospy.init_node('test_cartesian_trajectory') moveit_wrapper.setup() kin = boris_kinematics(root_link="left_arm_base_link") boris = BorisRobot(moveit_wrapper=moveit_wrapper) # trajectory = [-2.02830171585, 1.04696202278, 1.55436050892, -1.44723391533, -0.815707325935, 0.387918055058, -2.68925023079] joint_names = rospy.get_param("left_arm/joints") hand_joint_names = rospy.get_param("left_hand/joints") trajectories = [ 'suitcase_trajectory01.csv', 'cylinder_traj.csv', 'cylinder_slide_trajectory.csv', 'test_traj.csv', 'box_traj.csv', 'ibuprofen_trajectory.csv' ] trajectory, _ = parse_trajectory_file('contact_trajectories/' + trajectories[1]) traj_msg = make_ros_trajectory_msg(trajectory, joint_names, index_map=(1, 8)) traj_hand_msg = make_ros_trajectory_msg(trajectory, hand_joint_names[:1], index_map=(8, 9)) cartesian_traj = make_cartesian_trajectory(trajectory, index_map=(1, 8), fk_func=kin.forward_kinematics) boris.exit_control_mode() plan = boris.get_moveit_plan("left_arm", traj_msg.points[0].positions) plan_traj = plan.joint_trajectory t0 = rospy.Time.now() i = 0 n_wpts = len(plan_traj.points) total_time = plan_traj.points[-1].time_from_start.to_sec() frequency = n_wpts / total_time print "Frequency: ", frequency rate = rospy.Rate(frequency) boris.set_control_mode(mode="joint_impedance", limb_name="left_arm") cmd = None for joint_goal in (plan_traj.points): cmd = boris.cmd_joint_angles(angles=joint_goal.positions, execute=True) print "Time: ", ( rospy.Time.now() - t0).to_sec(), " Expect: ", joint_goal.time_from_start.to_sec() print "Cmd: ", cmd.data i += 1 # c = raw_input("next: ") if rospy.is_shutdown(): break rate.sleep() tf = rospy.Time.now() print "Final Time: ", (tf - t0).to_sec(), " Expect: ", trajectory[-1][0] t0 = rospy.Time.now() i = 0 n_wpts = len(traj_msg.points) total_time = traj_msg.points[-1].time_from_start.to_sec() frequency = n_wpts / total_time print "Frequency: ", frequency rate = rospy.Rate(frequency) cmd = None boris.follow_trajectory("left_hand", traj_hand_msg, first_waypoint_moveit=False) for joint_goal in (traj_msg.points): cmd = boris.cmd_joint_angles(angles=joint_goal.positions, execute=True) print "Time: ", ( rospy.Time.now() - t0).to_sec(), " Expect: ", joint_goal.time_from_start.to_sec() print "Cmd: ", cmd.data i += 1 # c = raw_input("next: ") if rospy.is_shutdown(): break rate.sleep() tf = rospy.Time.now() print "Final Time: ", (tf - t0).to_sec(), " Expect: ", trajectory[-1][0] rospy.sleep(3.0)
class JointTrajectoryActionServer(object): def __init__(self, limb, reconfig_server, rate=100.0, mode='joint_impedance', sim = False, interpolation='minjerk'): self._mode = mode self._dyn = reconfig_server self._ns = limb + '/joint_trajectory_controller' self._fjt_ns = self._ns + '/follow_joint_trajectory2' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._limb = BorisRobot() self._name = limb self._interpolation = interpolation self._server.start() self._alive = True # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() self._limb.set_control_mode(mode=self._mode) if sim: rospy.loginfo("Setting gains for simulated robot") self._limb.set_joint_impedance(np.array([2000,2000,2000,2000,2000,2000,2000]),np.array([100,100,100,100,100,100,100])) # Create our spline coefficients self._coeff = [None] * len(self._limb.joint_names(self._name)) # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(self._control_rate) def robot_is_enabled(self): return True#self._enable.state().enabled def clean_shutdown(self): self._alive = False self._limb.exit_control_mode() def _get_trajectory_parameters(self, joint_names, goal): # For each input trajectory, if path, goal, or goal_time tolerances # provided, we will use these as opposed to reading from the # parameter server/dynamic reconfigure # Goal time tolerance - time buffer allowing goal constraints to be met if goal.goal_time_tolerance: self._goal_time = goal.goal_time_tolerance.to_sec() else: self._goal_time = self._dyn.config['goal_time'] # Stopped velocity tolerance - max velocity at end of execution self._stopped_velocity = self._dyn.config['stopped_velocity_tolerance'] # Path execution and goal tolerances per joint for jnt in joint_names: if jnt not in self._limb.joint_names(self._name): rospy.logerr( "%s: Trajectory Aborted - Provided Invalid Joint Name %s" % (self._action_name, jnt,)) self._result.error_code = self._result.INVALID_JOINTS self._server.set_aborted(self._result) return # Path execution tolerance path_error = self._dyn.config[jnt + '_trajectory'] if goal.path_tolerance: for tolerance in goal.path_tolerance: if jnt == tolerance.name: if tolerance.position != 0.0: self._path_thresh[jnt] = tolerance.position else: self._path_thresh[jnt] = path_error else: self._path_thresh[jnt] = path_error # Goal error tolerance goal_error = self._dyn.config[jnt + '_goal'] if goal.goal_tolerance: for tolerance in goal.goal_tolerance: if jnt == tolerance.name: if tolerance.position != 0.0: self._goal_error[jnt] = tolerance.position else: self._goal_error[jnt] = goal_error else: self._goal_error[jnt] = goal_error def _get_current_position(self, joint_names): return [self._limb.joint_angle(joint) for joint in joint_names] def _get_current_velocities(self, joint_names): return [self._limb.joint_velocity(joint) for joint in joint_names] def _get_current_error(self, joint_names, set_point): current = self._get_current_position(joint_names) error = map(operator.sub, set_point, current) return zip(joint_names, error) def _update_feedback(self, cmd_point, jnt_names, cur_time): self._fdbk.header.stamp = rospy.Duration.from_sec(rospy.get_time()) self._fdbk.joint_names = jnt_names self._fdbk.desired = cmd_point self._fdbk.desired.time_from_start = rospy.Duration.from_sec(cur_time) self._fdbk.actual.positions = self._get_current_position(jnt_names) self._fdbk.actual.time_from_start = rospy.Duration.from_sec(cur_time) self._fdbk.error.positions = map(operator.sub, self._fdbk.desired.positions, self._fdbk.actual.positions ) self._fdbk.error.time_from_start = rospy.Duration.from_sec(cur_time) self._server.publish_feedback(self._fdbk) def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): if not self._alive: self._limb.exit_control_mode() elif self._mode == 'joint_impedance': pnt = JointTrajectoryPoint() pnt.positions = self._get_current_position(joint_names) if self._mode == 'joint_impedance' and self._alive: # zero inverse dynamics feedforward command pnt.velocities = [0.0] * len(joint_names) pnt.accelerations = [0.0] * len(joint_names) self._limb.cmd_joint_angles(pnt.positions, pnt.velocities, pnt.accelerations) def _command_joints(self, joint_names, point, start_time, dimensions_dict): if not self._alive: rospy.logerr("{0}: Robot arm in Error state. Stopping execution.".format( self._action_name)) self._limb.exit_control_mode() self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED self._server.set_aborted(self._result) return False elif self._server.is_preempt_requested(): rospy.logwarn("{0}: Trajectory execution Preempted. Stopping execution.".format( self._action_name)) self._limb.exit_control_mode() self._server.set_preempted() return False velocities = [] deltas = self._get_current_error(joint_names, point.positions) for delta in deltas: if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]] and self._path_thresh[delta[0]] >= 0.0)): rospy.logerr("%s: Exceeded Error Threshold on %s: %s" % (self._action_name, delta[0], str(delta[1]),)) self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED self._server.set_aborted(self._result) self._limb.exit_control_mode() return False # if self._mode == 'velocity': # velocities.append(self._pid[delta[0]].compute_output(delta[1])) if self._mode == 'joint_impedance': self._limb.cmd_joint_angles(point.positions, point.velocities, point.accelerations) return True def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict): pnt = JointTrajectoryPoint() pnt.time_from_start = rospy.Duration(cmd_time) num_joints = b_matrix.shape[0] pnt.positions = [0.0] * num_joints if dimensions_dict['velocities']: pnt.velocities = [0.0] * num_joints if dimensions_dict['accelerations']: pnt.accelerations = [0.0] * num_joints for jnt in range(num_joints): b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t) # Positions at specified time pnt.positions[jnt] = b_point[0] # Velocities at specified time if dimensions_dict['velocities']: pnt.velocities[jnt] = b_point[1] # Accelerations at specified time if dimensions_dict['accelerations']: pnt.accelerations[jnt] = b_point[-1] return pnt def _compute_bezier_coeff(self, joint_names, trajectory_points, dimensions_dict): # Compute Full Bezier Curve num_joints = len(joint_names) num_traj_pts = len(trajectory_points) num_traj_dim = sum(dimensions_dict.values()) num_b_values = len(['b0', 'b1', 'b2', 'b3']) b_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_b_values)) for jnt in xrange(num_joints): traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim)) for idx, point in enumerate(trajectory_points): current_point = list() current_point.append(point.positions[jnt]) if dimensions_dict['velocities']: current_point.append(point.velocities[jnt]) if dimensions_dict['accelerations']: current_point.append(point.accelerations[jnt]) traj_array[idx, :] = current_point d_pts = bezier.de_boor_control_pts(traj_array) b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts) return b_matrix def _get_minjerk_point(self, m_matrix, idx, t, cmd_time, dimensions_dict): pnt = JointTrajectoryPoint() pnt.time_from_start = rospy.Duration(cmd_time) num_joints = m_matrix.shape[0] pnt.positions = [0.0] * num_joints if dimensions_dict['velocities']: pnt.velocities = [0.0] * num_joints if dimensions_dict['accelerations']: pnt.accelerations = [0.0] * num_joints for jnt in range(num_joints): m_point = minjerk.minjerk_point(m_matrix[jnt, :, :, :], idx, t) # Positions at specified time pnt.positions[jnt] = m_point[0] # Velocities at specified time if dimensions_dict['velocities']: pnt.velocities[jnt] = m_point[1] # Accelerations at specified time if dimensions_dict['accelerations']: pnt.accelerations[jnt] = m_point[-1] return pnt def _compute_minjerk_coeff(self, joint_names, trajectory_points, point_duration, dimensions_dict): # Compute Full Minimum Jerk Curve num_joints = len(joint_names) num_traj_pts = len(trajectory_points) num_traj_dim = sum(dimensions_dict.values()) num_m_values = len(['a0', 'a1', 'a2', 'a3', 'a4', 'a5', 'tm']) m_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_m_values)) for jnt in xrange(num_joints): traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim)) for idx, point in enumerate(trajectory_points): current_point = list() current_point.append(point.positions[jnt]) if dimensions_dict['velocities']: current_point.append(point.velocities[jnt]) if dimensions_dict['accelerations']: current_point.append(point.accelerations[jnt]) traj_array[idx, :] = current_point m_matrix[jnt, :, :, :] = minjerk.minjerk_coefficients(traj_array, point_duration) return m_matrix def _determine_dimensions(self, trajectory_points): # Determine dimensions supplied position_flag = True velocity_flag = (len(trajectory_points[0].velocities) != 0 and len(trajectory_points[-1].velocities) != 0) acceleration_flag = (len(trajectory_points[0].accelerations) != 0 and len(trajectory_points[-1].accelerations) != 0) return {'positions':position_flag, 'velocities':velocity_flag, 'accelerations':acceleration_flag} def _on_trajectory_action(self, goal): joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points # Load parameters for trajectory self._get_trajectory_parameters(joint_names, goal) # Create a new discretized joint trajectory num_points = len(trajectory_points) if num_points == 0: rospy.logerr("%s: Empty Trajectory" % (self._action_name,)) self._server.set_aborted() return rospy.loginfo("%s: Executing requested joint trajectory" % (self._action_name,)) rospy.logdebug("Trajectory Points: {0}".format(trajectory_points)) for jnt_name, jnt_value in self._get_current_error( joint_names, trajectory_points[0].positions): if abs(self._path_thresh[jnt_name]) < abs(jnt_value): rospy.logerr(("{0}: Initial Trajectory point violates " "threshold on joint {1} with delta {2} radians. " "Aborting trajectory execution.").format( self._action_name, jnt_name, jnt_value)) self._server.set_aborted() return control_rate = rospy.Rate(self._control_rate) dimensions_dict = self._determine_dimensions(trajectory_points) # Force Velocites/Accelerations to zero at the final timestep # if they exist in the trajectory # Remove this behavior if you are stringing together trajectories, # and want continuous, non-zero velocities/accelerations between # trajectories if dimensions_dict['velocities']: trajectory_points[-1].velocities = [0.0] * len(joint_names) if dimensions_dict['accelerations']: trajectory_points[-1].accelerations = [0.0] * len(joint_names) # Compute Full Bezier Curve Coefficients for all 7 joints pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points] try: if self._interpolation == 'minjerk': # Compute Full MinJerk Curve Coefficients for all 7 joints point_duration = [pnt_times[i+1] - pnt_times[i] for i in range(len(pnt_times)-1)] m_matrix = self._compute_minjerk_coeff(joint_names, trajectory_points, point_duration, dimensions_dict) else: # Compute Full Bezier Curve Coefficients for all 7 joints b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict) except Exception as ex: rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}" " arm with error \"{2}: {3}\"").format( self._action_name, self._name, type(ex).__name__, ex)) self._server.set_aborted() return # Wait for the specified execution time, if not provided use now start_time = goal.trajectory.header.stamp.to_sec() if start_time == 0.0: start_time = rospy.get_time() while not rospy.get_time() >= start_time: pass # busy waiting rospy.sleep(0.002) # Loop until end of trajectory time. Provide a single time step # of the control rate past the end to ensure we get to the end. # Keep track of current indices for spline segment generation now_from_start = rospy.get_time() - start_time end_time = trajectory_points[-1].time_from_start.to_sec() while (now_from_start < end_time and not rospy.is_shutdown() and self.robot_is_enabled()): now = rospy.get_time() now_from_start = now - start_time idx = bisect.bisect(pnt_times, now_from_start) #Calculate percentage of time passed in this interval if idx >= num_points: cmd_time = now_from_start - pnt_times[-1] t = 1.0 elif idx >= 0: cmd_time = (now_from_start - pnt_times[idx-1]) t = cmd_time / (pnt_times[idx] - pnt_times[idx-1]) else: cmd_time = 0 t = 0 if self._interpolation == 'minjerk': point = self._get_minjerk_point(m_matrix, idx, t, cmd_time, dimensions_dict) else: point = self._get_bezier_point(b_matrix, idx, t, cmd_time, dimensions_dict) # Command Joint Position, Velocity, Acceleration command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict) self._update_feedback(point, joint_names, now_from_start) if not command_executed: return # Sleep to make sure the publish is at a consistent time control_rate.sleep() # Keep trying to meet goal until goal_time constraint expired last = trajectory_points[-1] last_time = trajectory_points[-1].time_from_start.to_sec() end_angles = dict(zip(joint_names, last.positions)) def check_goal_state(): for error in self._get_current_error(joint_names, last.positions): if (self._goal_error[error[0]] > 0 and self._goal_error[error[0]] < math.fabs(error[1])): return error[0] if (self._stopped_velocity > 0.0 and max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)]) > self._stopped_velocity): return False else: return True while (now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown()): #and self.robot_is_enabled() if not self._command_joints(joint_names, last, start_time, dimensions_dict): return now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) control_rate.sleep() now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) # Verify goal constraint result = check_goal_state() if result is True: rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" % (self._action_name, self._name)) self._result.error_code = self._result.SUCCESSFUL self._server.set_succeeded(self._result) elif result is False: rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" % (self._action_name, self._name)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) else: rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" % (self._action_name, result, self._name)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
class GraspExecuter(object): def __init__(self): self._moveit_wrapper = MoveitWrapper() self._moveit_wrapper.init_moveit_commander() rospy.init_node('grasp_player') self._moveit_wrapper.setup() self._kin = boris_kinematics(root_link="left_arm_base_link") self._boris = BorisRobot(moveit_wrapper=self._moveit_wrapper) self._scene = self._moveit_wrapper.scene() self._planning_frame = self._moveit_wrapper.robot().get_planning_frame( ) self._tf_buffer = tf2_ros.Buffer() self._listener = tf2_ros.TransformListener(self._tf_buffer) self._br = tf.TransformBroadcaster() self._scene.remove_world_object("table") self.add_table() self._solution = None self._grasp_waypoints = [] self._pre_grasp_plan = None self._pre_grasp_plan2 = None self._grasp_arm_joint_path = None self._grasp_hand_joint_path = None self._post_grasp_plan = None self._grasp_service_client = GraspServiceClient() self._boris.set_control_mode(mode="joint_impedance", limb_name="left_arm") self._arm_traj_generator = MinJerkTrajHelper() self._hand_traj_generator = MinJerkTrajHelper() self._pre_grasp_traj_generator = MinJerkTrajHelper() self._post_grasp_traj_generator = MinJerkTrajHelper() self._scan_waypoints = np.load( "/home/earruda/Projects/boris_ws/src/boris-robot/boris_tools/scripts/scan_waypoints2.npy" ) self._kbhit = KBHit() def scan_object(self): command = "rosservice call /grasp_service/acquire_cloud" self._boris.set_vel_accel_scaling("left_arm", 0.45, 0.45) for waypoint in self._scan_waypoints: # goto self._boris.goto_with_moveit("left_arm", waypoint) # scan process = subprocess.Popen(command.split(), stdout=subprocess.PIPE) output, error = process.communicate() print output if rospy.is_shutdown(): break self._boris.set_vel_accel_scaling("left_arm", 0.25, 0.25) def remove_collision_object(self, name): self._scene.remove_world_object(name) rospy.sleep(0.5) def add_table(self): self.remove_collision_object("table") p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self._planning_frame p.pose.position.x = 0.55 p.pose.position.y = 0 p.pose.position.z = -0.37 #-0.34 p.pose.orientation.w = 1.0 self._scene.add_box("table", p, (0.87, 1.77, 0.04)) rospy.sleep(2) def add_object_guard(self, grasp_solution): self.remove_collision_object("guard") # print "Got solution: ", grasp_solution cloud_centroid = grasp_solution['cloud_centroid'] min_pt = grasp_solution['cloud_min_pt'] max_pt = grasp_solution['cloud_max_pt'] p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self._planning_frame p.pose.position.x = cloud_centroid[0] p.pose.position.y = cloud_centroid[1] p.pose.position.z = cloud_centroid[2] p.pose.orientation.w = 1.0 self._scene.add_box("guard", p, (max_pt[0] - min_pt[0], max_pt[1] - min_pt[1], max_pt[2] - min_pt[2])) rospy.sleep(0.1) def get_solution(self): self._solution = self._grasp_service_client.get_grasp_solution() self.add_table() self.add_object_guard(self._solution) self._grasp_waypoints = solution2carthesian_path( self._solution, self._tf_buffer) group = self._moveit_wrapper.get_group("left_hand_arm") group.set_goal_joint_tolerance(0.001) # default 0.0001 group.set_goal_position_tolerance(0.01) # default 0.0001 group.set_goal_orientation_tolerance(0.001) # default 0.001 #self._grasp_waypoints[0].position.z += 0.15 dx = self._grasp_waypoints[1].position.x - self._grasp_waypoints[ 0].position.x dy = self._grasp_waypoints[1].position.y - self._grasp_waypoints[ 0].position.y dz = self._grasp_waypoints[1].position.z - self._grasp_waypoints[ 0].position.z dir_vec = np.array([dx, dy, dz]) dist = np.linalg.norm(dir_vec) dir_vec /= dist dir_vec *= 0.0 #0.05 target = geometry_msgs.msg.Pose() for n_tries in range(50): rand_offset_x = np.random.randn() * np.sqrt(0.0001) rand_offset_y = np.random.randn() * np.sqrt(0.0001) rand_offset_z = np.maximum( 0.15 + np.random.randn() * np.sqrt(0.0001), 0.10) target.position.x = self._grasp_waypoints[0].position.x - dir_vec[ 0] + rand_offset_x target.position.y = self._grasp_waypoints[0].position.y - dir_vec[ 1] + rand_offset_y print rand_offset_x, rand_offset_y, rand_offset_z target.position.z = self._grasp_waypoints[ 0].position.z + rand_offset_z target.orientation = self._grasp_waypoints[0].orientation self._pre_grasp_plan = self._boris.get_moveit_cartesian_plan( "left_hand_arm", target) is_executable = len( self._pre_grasp_plan.joint_trajectory.points) > 0 if is_executable: break # if is_executablediag_add: # # current_angles = self._boris.angles("left_arm") # # joint_positions = self._pre_grasp_plan.joint_trajectory.points[-1].positions # # self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), joint_positions) # # rospy.sleep(1.0) # self._grasp_arm_joint_path, fraction = self._boris.compute_cartesian_path_moveit("left_hand_arm",self._grasp_waypoints[:3]) # # Set back to current # # self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), current_angles) # is_executable = fraction >= 0.85 if is_executable: self._pre_grasp_pose = msg2Transform3(self._grasp_waypoints[0]) print "Hand joints %d ", len( self._pre_grasp_plan.joint_trajectory.points) self._pre_grasp_traj_generator.set_waypoints( self._pre_grasp_plan.joint_trajectory) rospy.loginfo( "Pre grasp plan length %d" % (len(self._pre_grasp_plan.joint_trajectory.points), )) rospy.loginfo("Pre grasp plan total time %f" % (self._pre_grasp_plan.joint_trajectory.points[-1]. time_from_start.to_sec(), )) # state = self._moveit_wrapper.robot().get_current_state() # print "Robot state" # print state #group = self._moveit_wrapper.get_group("left_hand_arm") return is_executable def update_step(self, step, time_steps, incr): step = step + incr if step >= len(time_steps): step = len(time_steps) - 1 elif step < 0: step = 0 return step def goto_pregrasp(self): self._boris.execute_moveit_plan("left_hand_arm", self._pre_grasp_plan) def plan_grasp(self): self.remove_collision_object("guard") self.remove_collision_object("table") # self._pre_grasp_plan2, fraction2 = self._boris.compute_cartesian_path_moveit("left_hand_arm",[self._grasp_waypoints[:1]]) self._grasp_arm_joint_path, fraction = self._boris.compute_cartesian_path_moveit( "left_hand_arm", self._grasp_waypoints[:3], eef_step=0.01, jump_threshold=50.0) # Set back to current # self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), current_angles) is_executable = fraction >= 0.95 ## try to plan final goal in case cartesian path if not is_executable: rospy.logwarn( "Failed to plan cartesian path passing through all waypoints. Trying last one only." ) self._grasp_arm_joint_path = self._boris.get_moveit_cartesian_plan( "left_hand_arm", self._grasp_waypoints[2]) is_executable = len( self._grasp_arm_joint_path.joint_trajectory.points) > 0 if not is_executable: rospy.logwarn( "Grasp not executable, maybe try again or try new grasp") else: self._grasp_arm_joint_path = self._grasp_arm_joint_path.joint_trajectory self._arm_traj_generator.set_waypoints(self._grasp_arm_joint_path) rospy.loginfo("Grasp path length %d" % (len(self._grasp_arm_joint_path.points), )) rospy.loginfo("Grasp path total time %f" % (self._grasp_arm_joint_path.points[-1]. time_from_start.to_sec(), )) total_time = self._grasp_arm_joint_path.points[ -1].time_from_start.to_sec() self._grasp_hand_joint_path = solution2hand_joint_path( self._solution, self._boris, total_time) print "Trajectory hand ", len( self._solution['joint_trajectory'][0]) self._hand_traj_generator.set_waypoints( self._grasp_hand_joint_path) rospy.loginfo("Hand path length %d" % (len(self._grasp_hand_joint_path.points), )) rospy.loginfo("Hand path total time %f" % (self._grasp_hand_joint_path.points[-1]. time_from_start.to_sec(), )) return is_executable def plan_post_grasp(self): self.remove_collision_object("guard") self.remove_collision_object("table") self._grasp_waypoints[3].position.z += 0.08 self._post_grasp_plan = self._boris.get_moveit_cartesian_plan( "left_hand_arm", self._grasp_waypoints[3]) # Set back to current # self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), current_angles) is_executable = len(self._post_grasp_plan.joint_trajectory.points) > 0 if not is_executable: rospy.logwarn( "Grasp not executable, maybe try again or try new grasp") else: self._post_grasp_plan = self._post_grasp_plan.joint_trajectory self._post_grasp_traj_generator.set_waypoints( self._post_grasp_plan) rospy.loginfo("Grasp path length %d" % (len(self._post_grasp_plan.points), )) rospy.loginfo( "Grasp path total time %f" % (self._post_grasp_plan.points[-1].time_from_start.to_sec(), )) return is_executable def execute_pre_grasp(self): rospy.loginfo("Pre Grasp Execution!!") key = None time_steps = np.linspace(0.0, 1.0, 600) step = 0 def exec_step(step): joint_goal = self._pre_grasp_traj_generator.get_point_t( time_steps[step]) print "CurrArmState:", self._boris.angles('left_arm') print "ArmGoal[%.2f]: " % ( time_steps[step], ), joint_goal.time_from_start.to_sec( ), " step=", step, " pos: ", joint_goal.positions cmd = self._boris.cmd_joint_angles(angles=joint_goal.positions, execute=True) # self._boris.goto_joint_angles('left_hand',hand_goal.positions) loop_rate = rospy.Rate(30.0) play_forward = False play_backward = False while not rospy.is_shutdown() and key != 'q': if self._kbhit.kbhit(): key = self._kbhit.getch() if key == '.': rospy.loginfo("Step %d" % (step, )) step = self.update_step(step, time_steps, 1) exec_step(step) if key == ',': step = self.update_step(step, time_steps, -1) exec_step(step) rospy.loginfo("Step %d" % (step, )) if key == ']': play_forward = True play_backward = False rospy.loginfo("Playing forward") elif key == '[': play_forward = False play_backward = True rospy.loginfo("Playing backward") elif key == 'p' and (play_forward or play_backward): play_forward = False play_backward = False rospy.loginfo("Halt open loop playing") if play_forward: step = self.update_step(step, time_steps, 1) exec_step(step) rospy.loginfo("Step %d" % (step, )) elif play_backward: step = self.update_step(step, time_steps, -1) exec_step(step) rospy.loginfo("Step %d" % (step, )) loop_rate.sleep() rospy.loginfo("Leaving Pre Grasp Execution!!") def execute_grasp(self): rospy.loginfo("Grasp Execution!!") key = None rospy.loginfo("Grasp path length %d" % (len(self._grasp_arm_joint_path.points), )) rospy.loginfo( "Grasp path total time %f" % (self._grasp_arm_joint_path.points[-1].time_from_start.to_sec(), )) time_steps = np.linspace(0.0, 1.0, 300) time_steps_arm = np.linspace(0.0, 1.0, 100) step = 0 step_arm = 0 def step_grasp(step, arm_step): joint_goal = self._arm_traj_generator.get_point_t( time_steps_arm[arm_step]) hand_goal = self._hand_traj_generator.get_point_t(time_steps[step]) print "HandGoal: ", hand_goal.time_from_start.to_sec( ), " step=", step, " pos: ", hand_goal.positions print "CurrArmState:", self._boris.angles('left_arm') print "ArmGoal: ", hand_goal.time_from_start.to_sec( ), " step=", step, " pos: ", joint_goal.positions cmd = self._boris.cmd_joint_angles(angles=joint_goal.positions, execute=True) self._boris.goto_joint_angles('left_hand', hand_goal.positions, hand_goal.time_from_start.to_sec()) loop_rate = rospy.Rate(30.0) play_forward = False play_backward = False while not rospy.is_shutdown() and key != 'q': key = None if self._kbhit.kbhit(): key = self._kbhit.getch() if key == '.': step = self.update_step(step, time_steps, 1) step_arm = self.update_step(step_arm, time_steps_arm, 1) rospy.loginfo("Step %d" % (step, )) step_grasp(step, step_arm) if key == ',': step = self.update_step(step, time_steps, -1) step_arm = self.update_step(step_arm, time_steps_arm, -1) rospy.loginfo("Step %d" % (step, )) step_grasp(step, step_arm) wrench = self._boris.wrench() force = np.array([ wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.force.z ]) # If force is larger than 5 we stop force_norm = np.linalg.norm(force) if force_norm >= 15.0: key = '[' rospy.logwarn("External forces too high %.3f" % (force_norm, )) # else: # rospy.loginfo("External forces %.3f"%(force_norm,)) if key == ']': play_forward = True play_backward = False rospy.loginfo("Playing forward") elif key == '[': play_forward = False play_backward = True rospy.loginfo("Playing backward") elif key == 'p' and (play_forward or play_backward): play_forward = False play_backward = False rospy.loginfo("Halt open loop playing") elif key == 'c': self._boris.goto_joint_angles('left_hand', [1.0], 0.01) elif key == 'o': self._boris.goto_joint_angles('left_hand', [0.0], 0.01) if play_forward: step = self.update_step(step, time_steps, 1) step_arm = self.update_step(step_arm, time_steps_arm, 1) step_grasp(step, step_arm) rospy.loginfo("Step %d" % (step, )) elif play_backward: step = self.update_step(step, time_steps, -1) step_arm = self.update_step(step_arm, time_steps_arm, -1) step_grasp(step, step_arm) rospy.loginfo("Step %d" % (step, )) loop_rate.sleep() rospy.loginfo("Leaving Grasp Execution!!") def step_execution(self, step, time_steps, trajectory_generator, command_func): joint_goal = trajectory_generator.get_point_t(time_steps[step]) command_func(joint_goal.positions) def execute_post_grasp(self): rospy.loginfo("Post Grasp Execution!!") key = None time_steps = np.linspace(0.0, 1.0, 200) #time_steps_arm = np.linspace(0.0,1.0,100) step = 0 loop_rate = rospy.Rate(30.0) play_forward = False play_backward = False while not rospy.is_shutdown() and key != 'q': key = None if self._kbhit.kbhit(): key = self._kbhit.getch() if key == '.': step = self.update_step(step, time_steps, 1) rospy.loginfo("Step %d" % (step, )) self.step_execution(step, time_steps, self._post_grasp_traj_generator, self._boris.cmd_joint_angles) if key == ',': step = self.update_step(step, time_steps, -1) rospy.loginfo("Step %d" % (step, )) self.step_execution(step, time_steps, self._post_grasp_traj_generator, self._boris.cmd_joint_angles) if key == ']': play_forward = True play_backward = False rospy.loginfo("Playing forward") elif key == '[': play_forward = False play_backward = True rospy.loginfo("Playing backward") elif key == 'p' and (play_forward or play_backward): play_forward = False play_backward = False rospy.loginfo("Halt open loop playing") if play_forward: step = self.update_step(step, time_steps, 1) self.step_execution(step, time_steps, self._post_grasp_traj_generator, self._boris.cmd_joint_angles) rospy.loginfo("Step %d" % (step, )) elif play_backward: step = self.update_step(step, time_steps, -1) self.step_execution(step, time_steps, self._post_grasp_traj_generator, self._boris.cmd_joint_angles) rospy.loginfo("Step %d" % (step, )) loop_rate.sleep() rospy.loginfo("Leaving Post Grasp Execution!!") def run(self): rospy.loginfo("Running!!") loop_rate = rospy.Rate(30) has_solution = False has_plan = False has_post_grasp_plan = False while not rospy.is_shutdown(): key = self._kbhit.getch() if key == "0": has_solution = self.get_solution() rospy.loginfo("Queried solution %s" % (has_solution, )) elif key == "1" and has_solution: #self.goto_pregrasp() self.execute_pre_grasp() elif key == "2" and has_solution: if has_plan: self.execute_grasp() else: rospy.logwarn( "No grasp plan has been generated. Press 9 to attempt to generate one." ) elif key == "3" and has_solution: if has_post_grasp_plan: self.execute_post_grasp() else: rospy.logwarn( "No post-grasp plan has been generated. Press 8 to attempt to generate one." ) elif key == "9": has_plan = self.plan_grasp() elif key == "8": has_post_grasp_plan = self.plan_post_grasp() elif key == "r": self.remove_collision_object("table") self.remove_collision_object("guard") elif key == "t": self.add_table() elif key == "g" and has_solution: self.add_object_guard(self._solution) elif key == "e": self.remove_collision_object("guard") elif key == "s": self.scan_object() elif not has_solution: rospy.logwarn("No grasp solution. Press 0.") loop_rate.sleep()
def __init__(self): self._robot = BorisRobot()
from boris_tools.trajectory_io import parse_trajectory_file, make_ros_trajectory_msg, make_cartesian_trajectory from boris_tools.boris_robot import BorisRobot from boris_tools.boris_kinematics import boris_kinematics from boris_tools.cartesian_imp_commander import CartesianImpedanceCommander if __name__ == '__main__': moveit_wrapper = MoveitWrapper() moveit_wrapper.init_moveit_commander() rospy.init_node('test_cartesian_trajectory') moveit_wrapper.setup() kin = boris_kinematics(root_link="left_arm_base_link") boris = BorisRobot(moveit_wrapper = moveit_wrapper) # trajectory = [-2.02830171585, 1.04696202278, 1.55436050892, -1.44723391533, -0.815707325935, 0.387918055058, -2.68925023079] joint_names = rospy.get_param("left_arm/joints") hand_joint_names = rospy.get_param("left_hand/joints") trajectories = ['suitcase_trajectory01.csv', 'cylinder_traj.csv', 'cylinder_slide_trajectory.csv', 'test_traj.csv', 'box_traj.csv', 'ibuprofen_trajectory.csv'] trajectory, _ = parse_trajectory_file('contact_trajectories/'+trajectories[5]) traj_msg = make_ros_trajectory_msg(trajectory,joint_names, index_map=(1,8))