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