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 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)
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)
from boris_tools.moveit_wrapper import MoveitWrapper
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])