def test_gripper():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
larm = arm.Arm('left', sim=sim)
# rarm.close_gripper()
# larm.close_gripper()
rarm.open_gripper()
larm.open_gripper()
def test_mantis():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
larm = arm.Arm('left', sim=sim)
rarm.go_to_posture('mantis', block=False)
larm.go_to_posture('mantis', block=False)
rarm.open_gripper()
larm.open_gripper()
def __init__(self,
object_material,
floor_material='None',
pressure_threshold=10000):
self.larm = arm.Arm('left', default_speed=0.08)
self.rarm = arm.Arm('right', default_speed=0.08)
self.pressure_sub = rospy.Subscriber('/pressure/l_gripper_motor',
PressureState,
self._pressure_callback)
self.pressure_threshold = pressure_threshold
self.reset(object_material, floor_material)
def test_arm():
a = arm.Arm('right')
p = a.get_pose()
print('Pose : {0}'.format(p))
home_pose = tfx.pose([0.48, -0.67, 0.85],
tfx.tb_angles(0, 0, 0),
frame='base_link')
a.go_to_pose(home_pose)
IPython.embed()
def test_home_pose():
a = arm.Arm('left')
# push home_pose
home_pose = tfx.pose([0.54, 0.2, 0.71],
tfx.tb_angles(-90, 0, 0),
frame='base_link')
home_joints = [
0.6857, 0.31154, 2.21, -1.062444, -0.33257, -1.212881, -0.81091
]
a.go_to_joints(home_joints)
IPython.embed()
def test_planner():
arm_name = 'right'
sim = simulator.Simulator(view=False)
a = arm.Arm(arm_name, sim=sim)
p = planner.Planner(arm_name, sim=sim, interact=False)
a.go_to_posture('untucked', speed=.2)
current_joints = a.get_joints()
current_pose = a.get_pose()
target_pose = current_pose + [.2, 0, 0]
print('Calling trajopt')
joint_traj = p.get_joint_trajectory(current_joints, target_pose)
print('\ndesired end pose:\n{0}'.format(target_pose.matrix))
print('joint_traj end pose:\n{0}'.format(a.fk(joint_traj[-1]).matrix))
IPython.embed()
def __init__(self):
#self.grasp_traj = None
#self.return_grasp_traj = None
#self.grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/grasp_trajectory', gm.PoseArray, self._grasp_traj_callback)
#self.return_grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/return_grasp_trajectory', gm.PoseArray, self._return_grasp_traj_callback)
self.grasp_joint_traj = None
self.return_grasp_joint_traj = None
self.grasp_joint_traj_sub = rospy.Subscriber(
'/check_handle_grasp/grasp_joint_trajectory', tm.JointTrajectory,
self._grasp_joint_traj_callback)
self.return_grasp_joint_traj_sub = rospy.Subscriber(
'/check_handle_grasp/return_grasp_joint_trajectory',
tm.JointTrajectory, self._return_grasp_joint_traj_callback)
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm('right', sim=self.sim)
self.speed = 0.06
def __init__(self):
self.arm_name = 'right'
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm(self.arm_name, sim=self.sim)
self.planner = planner.Planner(self.arm_name,
sim=self.sim,
interact=False)
self.sim.env.Load('../envs/SDHtable.xml')
#self.arm.go_to_posture('mantis')
self.home_pose = self.arm.get_pose()
self.handle_pose_sub = rospy.Subscriber('/localization/handle_pose',
geom_msg.PoseStamped,
self._handle_pose_callback)
self.handle_pose_callback, self.handle_pose = None, None
self.handle_pose_pub = rospy.Publisher('/debug_handle_pose',
geom_msg.PoseStamped)
rospy.loginfo('GreedyGrasp initialized')
def test_joints():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
larm = arm.Arm('left', sim=sim)
print('rarm joints: {0}'.format(rarm.get_joints()))