def move(self, x, y, z, orientation_matrix=np.eye(3)):
joint_states = self.chain.inverse_kinematics(
geometry_utils.to_transformation_matrix([x, y, z],
orientation_matrix))
# joint_states = self.chain.inverse_kinematics([ [-.5, 0, 0, x],
# [0, 0.5, 0, y],
# [0, 0, 0.5, z],
# [0, 0, 0, -.5] ])
print(joint_states)
hdr = Header()
hdr.seq = self.seq = self.seq + 1
hdr.stamp = rospy.Time.now()
hdr.frame_id = "My-Kinematic-Thingy"
js = JointState()
js.header = hdr
js.name = [
"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6",
"Joint_Gripper"
] #, ,
# "Joint_Grip_Servo", "Joint_Tip_Servo", "Joint_Grip_Servo_Arm",
# "Joint_Grip_Idle", "Joint_Tip_Idle", "Joint_Grip_Idle_Arm"]
js.position = joint_states[1:8]
js.velocity = []
js.effort = []
self.publisher.publish(js)
class ArmEnv3D():
def __init__(self, arm_obj = _left_arm_chain, actions = [-5.0,-0.5,0,0.5,5.0], limits = [(-20.0,100.0),(0.0,130.0),(0.0,130.0)], goal_ratio_percent = 5):
self._ARM = arm_obj
self._ACTIONS = [math.radians(a) for a in actions]
self._ACTION_DIM = len(arm_obj.links)-1
self._STATE_DIM = 3
self.STATE = np.zeros(self._STATE_DIM)
self.TARGET = np.zeros(self._STATE_DIM)
self.DONE = False
self.STEPS = 0
self.MAX_STEPS = 500
self.LIMITS = [(math.radians(lim[0]),math.radians(lim[1])) for lim in limits]
self.GOAL_RATIO = [np.around(np.interp(goal_ratio_percent, [0,100], (0,lim[1]-lim[0])), decimals=2) for lim in limits]
# Definimos la funcion pos2ang(pos) y ang2pos(ang)
pos2ang = lambda self,pos : list(self._ARM.inverse_kinematics(geometry_utils.to_transformation_matrix(pos))[:self._ACTION_DIM])
ang2pos = lambda self,ang : list(self._ARM.forward_kinematics(list(ang)+[0])[:3, 3])
# Definimos la funcion isInsLim(pos) y isReachable(pos), esta verifica que una posicion este dentro de los limites
isInsLim = lambda self,pos : np.all([True if lim[0] <= np.round(self.pos2ang(pos)[i],decimals=2) <= lim[1] else False for i,lim in enumerate(self.LIMITS)])
isReachable = lambda self,pos: True if np.all(np.round(self.ang2pos(self.pos2ang(pos)),decimals=2) == np.round(pos,decimals=2)) else False
# asCartesian(rtp) Convierte una posicion esferica (r,theta,phi) en cartesiana (x,y,z)
# asSpherical(xyz) Convierte una posicion cartesiana (x,y,z) en esferica (r,theta,phi)
asCartesian = lambda self,rtp: [rtp[0]*math.sin(rtp[1])*math.cos(rtp[2]),rtp[0]*math.sin(rtp[1])*math.sin(rtp[2]),rtp[0]*math.cos(rtp[1])]
asSpherical = lambda self,xyz: [np.sqrt(np.sum(np.square(xyz))),math.acos(xyz[2]/np.sqrt(np.sum(np.square(xyz)))),math.atan2(xyz[1],xyz[0])]
# Funcion reset(), devuelve el estado inicial
def reset(self):
stateok = False
st=np.zeros((2,3))
while not stateok:
# Crea 2 estados, el inicial y el objetivo
st[0] = self.ang2pos(np.radians([random.randrange(int(lim[0]), int(lim[1])) for lim in np.degrees(self.LIMITS)]))
st[1] = self.ang2pos(np.radians([random.randrange(int(lim[0]), int(lim[1])) for lim in np.degrees(self.LIMITS)]))
stateok = np.all([self.isReachable(st[0]),self.isInsLim(st[0]),self.isReachable(st[1]),self.isInsLim(st[1])])
st=np.round(st,decimals=2)
self.STATE = self.asSpherical(st[0])
self.TARGET = self.asSpherical(st[1])
return self.STATE
def encodeAction(self, action , numOfActions, numOfOutputs):
return [int((action/numOfActions**i)%numOfActions) for i in range(numOfOutputs)]
# Funcion step(action),
def step(self,action):
action_index = self.encodeAction(action,len(self._ACTIONS),self._ACTION_DIM)
actList = [self._ACTIONS[i] for i in action_index]
print(np.degrees(actList))
tmpState = self.ang2pos(np.array(self.pos2ang(self.asCartesian(self.STATE)))+np.array(actList))
distance = np.array(self.asCartesian(self.TARGET))-np.array(self.asCartesian(self.STATE))
reward = -np.sqrt(np.sum(np.square(distance)))
print(np.degrees(self.pos2ang(tmpState)))
if self.isInsLim(np.round(tmpState,decimals=2)) and self.isReachable(np.round(tmpState,decimals=2)):
self.STATE = self.asSpherical(tmpState)
self.DONE = np.all([abs(d)<=self.GOAL_RATIO[i] for i,d in enumerate(distance)])
return self.STATE,reward,self.DONE
else:
return self.STATE,reward-10,True
def xyz_to_servo_range(self, xyz, current_servo_monotony):
"""
Converts 3D cartesian centimeter coordinates to servo values in [500, 2500].
:param xyz: Array of 3 elements of a 3D cartesian systems of centimeters.
:param current_servo_monotony: List of 6 positive or negative servo rotation directions.
:return: List of 6 servo values in [500, 2500].
"""
k = self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(xyz, np.eye(3)))
k = np.multiply(k, np.negative(current_servo_monotony))
return self.radians_to_servo_range(k)
def xyz_to_servo_range2(xyz, current_servo_monotony):
k = le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(xyz, np.eye(3)))
k = np.multiply(k, current_servo_monotony)
return radians_to_servo_range(k)
# print("init_servo_radians: ", init_servo_radians)
#
# print("init_servo_radians: ", np.multiply(servo_range_to_radians(init_servo_values),
# current_servo_monotony[::-1]))
#
# init_position2 = le_arm_chain.forward_kinematics(init_servo_radians)
# init_position = np.round(servo_range_to_xyz2(init_servo_values, current_servo_monotony), 2)
# print("predicted_init_position: ", init_position)
# print("proper init_position: ", target_position)
except Exception as e:
print("Exception: {}".format(str(e)))
if center_init:
print("Top position (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
init_position,
np.eye(3))))
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
init_position,
np.eye(3))), ax, target=init_position)
matplotlib.pyplot.show()
print("Target angles (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
target_position,
np.eye(3))))
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
target_position,
def move_arm(self,
target_position,
last_servo_locations,
trajectory_steps=-1):
"""
Gradually moves the end-effector of the robotic arm, from the latest known servo positions, to a desired
3D centimeter cartesian position.
:param target_position: List of 3 values, the desired end-effector, 3D centimeter cartesian position.
:param last_servo_locations: List of the latest 6 servo values in [500, 2500].
:param trajectory_steps: Scalar integer, the total steps for the end effector trajectory.
:return: True if successfully move the arm.
"""
action_successful = False
last_servo_values = self.init_position
if trajectory_steps == -1:
trajectory_steps = self.control_world_model["trajectory_steps"]
# TODO: move last position to world model
if self.control_world_model[
"detect_last_position"]: # TODO: get last servo values, world may be old...?
last_servo_values = last_servo_locations
try:
if self.control_world_model["send_requests"]:
url = "http://{}/".format(self.arm_url)
r = requests.get(url, data="")
if r.status_code == 200:
result = r.json()["variables"]
last_servo_values = np.array([
result["servo6"], result["servo5"],
result["servo4"], result["servo3"],
result["servo2"], result["servo1"]
])
if self.control_world_model["verbose"]:
print("last_servo_values: ", last_servo_values)
print(
"last_servo_xyz",
np.round(
self.servo_range_to_xyz(
last_servo_values,
self.control_world_model[
"current_servo_monotony"]), 2))
except Exception as e_pos:
print("Exception: {}".format(str(e_pos)))
if self.control_world_model["center_init"]:
if self.control_world_model["verbose"]:
print(
"Top position (radians): ",
self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
self.init_position, np.eye(3))))
if self.control_world_model["show_plots"]:
ax = matplotlib.pyplot.figure().add_subplot(111,
projection='3d')
self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
self.init_position, np.eye(3))),
ax,
target=self.init_position)
matplotlib.pyplot.show()
init_position2 = self.init_position
init_angle_radians2 = self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(init_position2, np.eye(3)))
from_servo_range = self.radians_to_servo_range(init_angle_radians2)
if self.control_world_model["detect_last_position"]:
from_servo_range = last_servo_values
to_servo_range = self.xyz_to_servo_range(
target_position,
self.control_world_model["current_servo_monotony"])
kinematic_servo_range_trajectory = self.get_servo_range_trajectory(
from_servo_range, to_servo_range, trajectory_steps)
if self.control_world_model["verbose"]:
print(
"init_angle_radians2: {}, from_servo_range: {}, to_servo_range: {}, servo_range_trajectory: {}"
.format(init_angle_radians2, from_servo_range, to_servo_range,
kinematic_servo_range_trajectory))
if self.control_world_model["show_plots"]:
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
target_position, np.eye(3))),
ax,
target=target_position)
matplotlib.pyplot.show()
if self.control_world_model["send_requests"]:
action_successful = self.send_restful_trajectory_requests(
kinematic_servo_range_trajectory)
return action_successful
translation_vector=[0, 0, -0.98],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="ankle",
translation_vector=[0, 0, -1.13],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
],
active_links_mask=[False, True, True, True])
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
target_vector = [0.1, -0.2, 0.1]
l_wrist_matrix = geometry_utils.to_transformation_matrix(target_vector)
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
left_arm_start_position = left_arm_chain.forward_kinematics([0] * 4)
right_arm_start_position = right_arm_chain.forward_kinematics([0] * 4)
head_start_position = head_chain.forward_kinematics([0] * 3)
left_leg_start_position = left_leg_chain.forward_kinematics([0] * 4)
right_leg_start_position = right_leg_chain.forward_kinematics([0] * 4)
print(right_arm_start_position)
left_arm_chain.plot(left_arm_chain.inverse_kinematics(l_wrist_matrix),
ax,
target=target_vector)
right_arm_chain.plot(
name="shoulder",
translation_vector=[-10, 0, 5],
orientation=[0, 1.57, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="elbow",
translation_vector=[25, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="wrist",
translation_vector=[22, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
)
])
target_position = [2, 2, 2]
print(
left_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(target_position, np.eye(3))))
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
left_arm_chain.plot(left_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
[2, 2, 2], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])),
ax,
target=target_position)
matplotlib.pyplot.show()