hand_data['palm_yaw']
]
grab_strength = hand_data['grab_strength']
target_position = [x / 1000 for x in pos] # x, y, z
pos[0], pos[1], pos[2] = pos[2], pos[0], pos[1] # z x y to x y z
ori[0], ori[1], ori[2] = ori[2], -ori[0] + 3.14, ori[
1] # z y x to x y z
# Adjust the offset
target_position[0] -= 0.4
target_position[2] += 0.3
target_orientation = list(euler2quat(ori)) # w, x, y, z
# target_orientation = target_orientation[1:] + target_orientation[:1]
# Compute IK solution
goal_curr = blue.inverse_kinematics(target_position,
target_orientation)
# Send command to robot
if goal_curr != []:
goal = goal_curr
print("goal: ", goal)
blue.set_joint_positions(goal,
duration=3,
soft_position_control=False)
blue.command_gripper(grab_strength, 10.0, wait=False)
# Wait for system to settle
i += 1
time.sleep(3)
# Direct motor angle mapping approach
else:
# A basic example of sending Blue a command in cartesian space.
from blue_interface import BlueInterface
import numpy as np
import time
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
# Compute IK solution
target_position = [0.4, 0, 0] # x, y, z
target_orientation = [0.6847088, -0.17378805, -0.69229771,
-0.1472938] # x, y, z, w
target_joint_positions = blue.inverse_kinematics(target_position,
target_orientation)
# Send command to robot
blue.set_joint_positions(target_joint_positions, duration=5)
# Wait for system to settle
time.sleep(5)
# Print results
joint_positions = blue.get_joint_positions()
pose = blue.get_cartesian_pose()
print_aligned = lambda left, right: print("{:30} {}".format(
left, np.round(right, 4)))
print_aligned("Target joint positions: ", target_joint_positions)
print_aligned("End joint positions: ", joint_positions)
print_aligned("Target cartesian position:", target_position)