def pour_cup_general(self, vel=2, total_angle=3.14 * 2 / 3):
gripper_pos, gripper_quat = self.uc.return_cartesian_pose(
self.arm, 'base_link')
numsteps = 1
angles = []
total_time = total_angle / vel
new_gripper_quat = gripper_quat[:]
#TODO update new_gripper_quat
new_gripper_euler = list(euler_from_quaternion(new_gripper_quat))
new_gripper_euler[0] += total_angle
new_gripper_quat = list(
quaternion_from_euler(new_gripper_euler[0], new_gripper_euler[1],
new_gripper_euler[2]))
angles = arm_ik('r',
gripper_pos,
new_gripper_quat,
self.uc.get_torso_position(),
current=self.uc.get_arm_positions('r'))
angles = self.uc.adjust_config(angles, self.uc.get_arm_positions('r'))
times = [total_time]
angles = [angles]
self.uc.command_arm_trajectory('r',
angles,
times,
blocking=True,
logging=False)
def command_pose(uc, pose, arm, timeout=4, unwind=True):
angles = arm_ik('r',
pose[0],
pose[1],
uc.get_torso_position(),
current=uc.get_arm_positions('r'))
uc.command_arm('r', angles, timeout, blocking=True)
def command_pose(uc, pose, arm, timeout=4, unwind=True):
angles = arm_ik('r',
pose[0],
pose[1],
uc.get_torso_position(),
current=uc.get_arm_positions('r'))
if unwind:
angles = uc.adjust_config(angles, uc.get_arm_positions(arm))
uc.command_arm('r', angles, timeout, blocking=True)
rospy.sleep(8)
def test_moving(robot):
times = [1, 2]
gripper_pos, gripper_quat = robot.uc.return_cartesian_pose(
'r', 'base_link')
print(gripper_pos, "gripper pos")
pos_trajectory = [[[0.7, -0.3, 1], gripper_quat],
[gripper_pos, gripper_quat]]
robot.uc.lift_torso()
arm_traj = []
for pose in pos_trajectory:
angles = arm_ik('r',
pose[0],
pose[1],
robot.uc.get_torso_position(),
current=robot.uc.get_arm_positions('r'))
assert angles is not None
arm_traj.append(angles)
robot.uc.command_arm_trajectory('r', arm_traj, times, True)