def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
s = sin(joint_angle)
c = cos(joint_angle)
# Like it's stated in B-Human coding guide here we apply the 45 degree rotation of the LHipYawPitch and RHipYawPitch
if 'LHipYawPitch' == joint_name or 'RHipYawPitch' == joint_name:
if joint_name[0] == 'L':
T = np.dot(
T,
np.array([[1, 0, 0, 0],
[0, cos(np.pi / 4), -sin(np.pi / 4), 0],
[0, sin(np.pi / 4),
cos(np.pi / 4), 0], [0, 0, 0, 1]]))
if joint_name[0] == 'R':
T = np.dot(
T,
np.array([[1, 0, 0, 0],
[0, cos(-np.pi / 4), -sin(-np.pi / 4), 0],
[0, sin(-np.pi / 4),
cos(-np.pi / 4), 0], [0, 0, 0, 1]]))
# Differ between the joint angles for Roll, Pitch and Yaw movement
if 'Roll' in joint_name[-4:]:
T = np.dot(
T,
np.array([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0],
[0, 0, 0, 1]]))
elif 'Pitch' in joint_name[-5:]:
T = np.dot(
T,
np.array([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0],
[0, 0, 0, 1]]))
elif 'Yaw' in joint_name[-3:]:
T = np.dot(
T,
np.array([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]))
# I decided to put the x y z kords into the last row like it's done in the robot_arm_2d notebook
# even so its different shown on page 36 of the third lecture material
if joint_name in self.jointLengths.keys():
for i in range(3):
T[i, -1] = self.jointLengths[joint_name][i]
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = np.zeros([4, 4])
sin_vec = sin(joint_angle)
cos_vec = cos(joint_angle)
# Differ between the three possible movements angles (Roll, Pitch and Yaw)
if 'Roll' in joint_name:
T = matrix([[1, 0, 0, 0], [0, cos_vec, -sin_vec, 0],
[0, sin_vec, cos_vec, 0], [0, 0, 0, 1]])
if 'Pitch' in joint_name:
T = matrix([[cos_vec, 0, sin_vec, 0], [0, 1, 0, 0],
[-sin_vec, 0, cos_vec, 0], [0, 0, 0, 1]])
if 'Yaw' in joint_name:
T = matrix([[cos_vec, -sin_vec, 0, 0], [sin_vec, cos_vec, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
T[3, 0:3] = self.len_joint[joint_name]
return T
def calc_phi_points(points, laserpos, lasertheta):
"""Given an array of triples points that should be in the plane generated by a
laser at laserpos with theta lasertheta, calculate the inclination of the laser
plane's normal vector."""
plane_line = ddd.coord(-np.sin(lasertheta), np.cos(lasertheta), 0)
normals = np.cross(np.array(plane_line.T)[0], points - np.array(laserpos.T)[0])
return calc_phi_norm(np.average((normals.T / npl.norm(normals, axis = 1)).T, axis = 0), lasertheta)
def local_trans(self, joint_name, joint_angle, translation):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = sin(joint_angle)
c = cos(joint_angle)
x, y, z = translation
if 'Yaw' in joint_name:
T = matrix([[c, -s, 0.0, x], [s, c, 0.0, y], [0.0, 0.0, 1.0, z],
[0.0, 0.0, 0.0, 1.0]])
if 'Roll' in joint_name:
T = matrix([[1.0, 0.0, 0.0, x], [0.0, c, -s, y], [0.0, s, c, z],
[0.0, 0.0, 0.0, 1.0]])
if 'Pitch' in joint_name:
T = matrix([[c, 0.0, s, x], [0.0, 1.0, 0.0, y], [-s, 0.0, c, z],
[0.0, 0.0, 0.0, 1.0]])
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
R_x = identity(4)
R_y = identity(4)
R_z = identity(4)
sin_angle = sin(joint_angle)
cos_angle = cos(joint_angle)
x = self.link_offsets[joint_name][0]
y = self.link_offsets[joint_name][1]
z = self.link_offsets[joint_name][2]
if 'Roll' in joint_name:
R_x = matrix([[1, 0, 0, 0], [0, cos_angle, -sin_angle, 0],
[0, sin_angle, cos_angle, 0], [x, y, z, 1]])
elif 'Pitch' in joint_name:
R_y = matrix([[cos_angle, 0, sin_angle, 0], [0, 1, 0, 0],
[-sin_angle, 0, cos_angle, 0], [x, y, z, 1]])
elif 'Yaw' in joint_name:
R_z = matrix([[cos_angle, sin_angle, 0, 0],
[-sin_angle, cos_angle, 0, 0], [0, 0, 1, 0],
[x, y, z, 1]])
T = R_x * R_y * R_z
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
#T = array((4,4))
s = sin(joint_angle)
c = cos(joint_angle)
#x-Rotation
if joint_name in ['LShoulderRoll','LElbowRoll','RShoulderRoll','RElbowRoll','LHipRoll','LAnkleRoll','RHipRoll','RAnkleRoll']:
T = array([[1,0,0,0],
[0,c,-s,0],
[0,s,c,0],
[0,0,0,0]])
#y-Rotation
if joint_name in ['HeadPitch','LShoulderPitch','RShoulderPitch','LHipPitch','RHipPitch','LKneePitch','LAnklePitch','RKneePitch','RAnklePitch']:
T = array([[c,0,s,0],
[0,1,0,0],
[-s,0,c,0],
[0,0,0,0]])
#z-Rotation
if joint_name in ['HeadYaw','LElbowYaw','RElbowYaw']:
T = array([[c,s,0,0],
[-s,c,0,0],
[0,0,1,0],
[0,0,0,0]])
if joint_name in ['RHipYawPitch','LHipYawPitch']:
y = array([[c,0,s,0],
[0,1,0,0],
[-s,0,c,0],
[0,0,0,0]])
z = array([[c,s,0,0],
[-s,c,0,0],
[0,0,1,0],
[0,0,0,0]])
T = y.dot(z)
#print(joint_name)
#print(T)
T[3][0] = self.lenJoint.get(joint_name)[0]
T[3][1] = self.lenJoint.get(joint_name)[1]
T[3][2] = self.lenJoint.get(joint_name)[2]
T[3][3] = 1
return T
def sind(alpha):
"""
This function calculates the sine of an angle in degrees.
:param alpha: Angle in degrees.
:type alpha: float
:return: The sine of the angle.
:rtype: float
"""
return sin(np.deg2rad(alpha))
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
#print("Current joint: ", joint_name)
j_cos = cos(joint_angle)
j_sin = sin(joint_angle)
# standard transformations (rotations only)
# rotation around x-axis
Rx = [[1, 0, 0, 0], [0, j_cos, -j_sin, 0], [0, j_sin, j_cos, 0],
[0, 0, 0, 1]]
# rotation around y-axis
Ry = [[j_cos, 0, j_sin, 0], [0, 1, 0, 0], [-j_sin, 0, j_cos, 0],
[0, 0, 0, 1]]
# rotation around z-axis
Rz = [[j_cos, j_sin, 0, 0], [-j_sin, j_cos, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]
if joint_name in self.pitches:
T = Ry
elif joint_name in self.yaws:
T = Rz
elif joint_name in self.rolls:
T = Rx
elif joint_name in self.yaw_pitches:
# combine yaw and pitch
T = dot(Rz, Ry)
else:
print('ERROR: Unknown joint name "' + joint_name + '"')
# T stays identity
# add the offset in last collumn
T[0][3] = self.offsets[joint_name][0]
T[1][3] = self.offsets[joint_name][1]
T[2][3] = self.offsets[joint_name][2]
#print(np.round(T,2))
return T
def set_dq_pjoints(self, dq):
pjoints = l_from_l_of_l(self.pjoints)
for jnt in pjoints:
jnt.q += dq
# TODO: remainder by 2 * pi and check limits.
# Beware not to limit revolute joints if (q +/- 2*pi) would be
# within the limits.
if ((jnt.q < jnt.qmin) or (jnt.q > jnt.qmax)) and (jnt.isrevolute()):
from numpy.matlib import atan2, sin, cos
jnt.q = atan2(sin(jnt.q),cos(jnt.q))
if (jnt.q < jnt.qmin):
jnt.q = jnt.qmin
elif (jnt.q > jnt.qmax):
jnt.q = jnt.qmax
def rotation(self, axis, angle):
s = sin(angle)
c = cos(angle)
R_x = matrix([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]])
R_y = matrix([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]])
R_z = matrix([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
if axis == 'x':
return R_x
elif axis == 'y':
return R_y
elif axis == 'z':
return R_z
def calc_rot_matrix(posor):
"""Calculate the rotation matrix that takes a column vector from the camera
coordinates associated with posor to absolute coordinates"""
th = posor.theta
theta = np.mat([[np.cos(th), -np.sin(th), 0],
[np.sin(th), np.cos(th), 0],
[0, 0, 1]], dtype=npfloat)
ph = posor.phi
phi = np.mat([[np.cos(ph), 0, -np.sin(ph)],
[0, 1, 0],
[np.sin(ph), 0, np.cos(ph)]], dtype=npfloat)
ps = posor.psi
psi = np.mat([[1, 0, 0],
[0, np.cos(ps), -np.sin(ps)],
[0, np.sin(ps), np.cos(ps)]], dtype=npfloat)
return theta * phi * psi
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
s = sin(joint_angle)
c = cos(joint_angle)
# YOUR CODE HERE
#get axis length from dicct
x, y, z = self.joint_length[joint_name]
if ("HipYaw" in joint_name):
#special transformation due to 45 degree hip angle needs implementation
T = matrix([[c, -s, 0, x], [s, c, 0, y], [0, 0, 1, z],
[0, 0, 0, 1]])
elif ("Yaw" in joint_name):
# z transformation
T = matrix([[c, -s, 0, x], [s, c, 0, y], [0, 0, 1, z],
[0, 0, 0, 1]])
elif "Roll" in joint_name:
# x transformation
T = matrix([[1, 0, 0, x], [0, c, -s, y], [0, s, c, z],
[0, 0, 0, 1]])
elif "Pitch" in joint_name:
# y transformation
T = matrix([[c, 0, s, x], [0, 1, 0, y], [-s, 0, c, z],
[0, 0, 0, 1]])
else:
print("warn: unknown joint")
return T
import math
import numpy as np
import numpy.matlib as nm
import matplotlib.pyplot as plt
np.seterr(divide='ignore', invalid='ignore')
n = 200
a = nm.linspace(0, nm.pi, n / 2)
x_u = np.c_[nm.cos(a) + 0.5, nm.cos(a) - 0.5].reshape(n, 1)
u = -10 * x_u + nm.randn(n, 1)
x_v = np.c_[nm.sin(a), -nm.sin(a)].reshape(n, 1)
v = 10 * x_v + nm.randn(n, 1)
x = np.c_[u, v]
y = np.zeros((n, 1))
y[0] = 1
y[n - 1] = -1
x2 = np.sum(np.power(x, 2), 1)
hh = 2 * 1**2
k = nm.exp(-(nm.repmat(x2, 1, n) + nm.repmat(x2.T, n, 1) - 2 * x * x.T) / hh)
w = k
t_tmp1 = k**2 + 1 * np.eye(n) + 10 * k * (nm.diag(sum(w)) - w) * k
t = np.linalg.inv(t_tmp1) * (k * y)
m = 100
X = nm.linspace(-20, 20, m).T
X2 = np.power(X, 2)
U = nm.exp(
-(nm.repmat(np.power(u, 2), 1, m) + nm.repmat(X2.T, n, 1) - 2 * u * X.T) /
hh)
V = nm.exp(
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
j_cos = cos(joint_angle)
j_sin = sin(joint_angle)
Mx = [[ 1, 0, 0, 0],
[ 0, j_cos, -j_sin, 0],
[ 0, j_sin, j_cos, 0],
[ 0, 0, 0, 1]]
My = [[ j_cos, 0, j_sin, 0],
[ 0, 1, 0, 0],
[ -j_sin, 0, j_cos, 0],
[ 0, 0, 0, 1]]
Mz = [[ j_cos, j_sin, 0, 0],
[ -j_sin, j_cos, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]]
""" Pitch / Yaw / Roll"""
if joint_name in ['HeadPitch',
'LShoulderPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch',
'RShoulderPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch']:
T = My
elif joint_name in ['HeadYaw', 'LElbowYaw', 'LWristYaw', 'RElbowYaw', 'RWristYaw']:
T = Mz
elif joint_name in ['LShoulderRoll', 'LElbowRoll', 'LHipRoll', 'LAnkleRoll',
'RShoulderRoll', 'RElbowRoll', 'RHipRoll', 'RAnkleRoll']:
T = Mx
elif joint_name in ['LHipYawPitch', 'RHipYawPitch']:
T = dot(Mz, My)
"""joint length offset"""
offsets = {'HeadYaw' : [0.00, 0.00, 126.50],
'HeadPitch' : [0.00, 0.00, 0.00],
'LShoulderPitch' : [0.00, 98.00, 100.00],
'LShoulderRoll' : [0.00, 0.00, 0.00],
'LElbowYaw' : [105.00, 15.00, 0.00],
'LElbowRoll' : [0.00, 0.00, 0.00],
'LWristYaw' : [55.95, 0.00, 0.00],
'RShoulderPitch': [0.00, -98.00, 100.00],
'RShoulderRoll': [0.00, 0.00, 0.00],
'RElbowYaw': [105.00, -15.00, 0.00],
'RElbowRoll': [0.00, 0.00, 0.00],
'RWristYaw': [55.95, 0.00, 0.00],
'LHipYawPitch' : [0.00, 50.00, -85.00],
'LHipRoll' : [0.00, 0.00, 0.00],
'LHipPitch' : [0.00, 0.00, 0.00],
'LKneePitch' : [0.00, 0.00, -100.00],
'LAnklePitch' : [0.00, 0.00, -102.90],
'LAnkleRoll' : [0.00, 0.00, 0.00],
'RHipYawPitch': [0.00, -50.00, -85.00],
'RHipRoll': [0.00, 0.00, 0.00],
'RHipPitch': [0.00, 0.00, 0.00],
'RKneePitch': [0.00, 0.00, -100.00],
'RAnklePitch': [0.00, 0.00, -102.90],
'RAnkleRoll': [0.00, 0.00, 0.00]}
T[0][3] = offsets[joint_name][0]
T[1][3] = offsets[joint_name][1]
T[2][3] = offsets[joint_name][2]
return T
offset = tsid.robot.com(tsid.formulation.data())
amp = np.matrix([0.0, 0.05, 0.0]).T
two_pi_f = 2 * np.pi * np.matrix([0.0, 0.5, 0.0]).T
two_pi_f_amp = np.multiply(two_pi_f, amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
sampleCom = tsid.trajCom.computeNext()
samplePosture = tsid.trajPosture.computeNext()
t = 0.0
q, v = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
sampleCom.pos(offset + np.multiply(amp, matlib.sin(two_pi_f * t)))
sampleCom.vel(np.multiply(two_pi_f_amp, matlib.cos(two_pi_f * t)))
sampleCom.acc(np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f * t)))
tsid.comTask.setReference(sampleCom)
tsid.postureTask.setReference(samplePosture)
HQPData = tsid.formulation.computeProblemData(t, q, v)
# if i == 0: HQPData.print_all()
sol = tsid.solver.solve(HQPData)
if (sol.status != 0):
print "QP problem could not be solved! Error code:", sol.status
break
tau = tsid.formulation.getActuatorForces(sol)
pEE = offset.copy()
vEE = matlib.zeros((6, 1))
aEE = matlib.zeros((6, 1))
tsid.gui.addSphere('world/ee', conf.SPHERE_RADIUS, conf.EE_SPHERE_COLOR)
tsid.gui.addSphere('world/ee_ref', conf.REF_SPHERE_RADIUS,
conf.EE_REF_SPHERE_COLOR)
t = 0.0
q[:, 0], v[:, 0] = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
pEE[:3, 0] = offset[:3, 0] + np.multiply(
conf.amp, matlib.sin(conf.two_pi_f * t + conf.phi))
vEE[:3, 0] = np.multiply(two_pi_f_amp,
matlib.cos(conf.two_pi_f * t + conf.phi))
aEE[:3, 0] = np.multiply(two_pi_f_squared_amp,
-matlib.sin(conf.two_pi_f * t + conf.phi))
sampleEE.pos(pEE)
sampleEE.vel(vEE)
sampleEE.acc(aEE)
tsid.eeTask.setReference(sampleEE)
HQPData = tsid.formulation.computeProblemData(t, q[:, i], v[:, i])
# if i == 0: HQPData.print_all()
sol = tsid.solver.solve(HQPData)
if (sol.status != 0):
print "Time %.3f QP problem could not be solved! Error code:" % t, sol.status
import math
import numpy as np
import numpy.matlib as nm
import matplotlib.pyplot as plt
n = 100
u = nm.randn(n, 1) / 4 + 2
x = nm.randn(n, 1) / 2 + 1
w = 2 * nm.exp(-8 * np.power((x - 2), 2) + 2 * np.power((x - 1), 2))
y = nm.sin(nm.pi * x) / (nm.pi * x) + 0.1 * nm.randn(n, 1)
x2 = np.ones(len(x)).reshape(len(x), 1) # 生成一个维度为（n，1），元素全是1的数组
x = np.c_[x, x2] # x(:,2)=1;加第二行为1
t1 = np.multiply(nm.repmat(w, 1, 2), x) # np.multiply为矩阵对应元素相乘
t = np.linalg.inv(x.T * t1) * (x.T *
(np.multiply(w, y))) # np.linalg.inv(.)求逆矩阵
X = nm.linspace(-1, 3, 100) # 生成数据节点
Y = nm.sin(nm.pi * X) / (nm.pi * X) # 根据节点计算Y
u = np.c_[u, x2]
v = u * t
print(u.shape)
plt.figure()
plt.plot(x[:, 0], y, 'bo', label='xi,yi') # 绘制原始数据点
plt.plot(X, Y, 'r-', label='f(x)') # 绘制重要度加权的最小二乘曲线
plt.plot(u[:, 0], v, 'kx', label='xi1,yi1') # 绘制最小二乘曲线
plt.legend() # 显示绘制的legend
plt.show() # 显示绘制的画布
def calc_phi_norm(norm, lasertheta):
"""Given norm, the normal vector to the plane of the laser, and lasertheta, the
angle it is rotated along z from the positive x-axis, calculate phi, the angle
of inclination of the norm."""
return np.arctan2(norm[2], norm[0]*np.cos(lasertheta) + norm[1]*np.sin(lasertheta))
amp = np.matrix([0.2, 0.3, 0.4, 0.0, 0.0, 0.0]).T # amplitude
phi = np.matrix([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0]).T # phase
two_pi_f = 2 * np.pi * np.matrix([1.0, 0.5, 0.3, 0.0, 0.0, 0.0
]).T # frequency (time 2 PI)
two_pi_f_amp = np.multiply(two_pi_f, amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
t = 0.0
dt = conf.dt
q[:, 0], v[:, 0] = q0, v0
for i in range(0, N):
time_start = time.time()
# set reference trajectory
q_ref[:, i] = q0 + np.multiply(amp, matlib.sin(two_pi_f * t + phi))
v_ref[:, i] = np.multiply(two_pi_f_amp, matlib.cos(two_pi_f * t + phi))
dv_ref[:, i] = np.multiply(two_pi_f_squared_amp,
-matlib.sin(two_pi_f * t + phi))
samplePosture.pos(q_ref[:, i])
samplePosture.vel(v_ref[:, i])
samplePosture.acc(dv_ref[:, i])
postureTask.setReference(samplePosture)
HQPData = formulation.computeProblemData(t, q[:, i], v[:, i])
sol = solver.solve(HQPData)
if (sol.status != 0):
print("Time %.3f QP problem could not be solved! Error code:" % t,
sol.status)
break
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
ja_sin = sin(joint_angle)
ja_cos = cos(joint_angle)
mRange = {
'HeadYaw': [0.00, 0.00, 126.50],
'HeadPitch': [0.00, 0.00, 0.00],
'LShoulderPitch': [0.00, 98.00, 100.00],
'LShoulderRoll': [0.00, 0.00, 0.00],
'LElbowYaw': [105.00, 15.00, 0.00],
'LElbowRoll': [0.00, 0.00, 0.00],
'LWristYaw': [55.95, 0.00, 0.00],
'RShoulderPitch': [0.00, -98.00, 100.00],
'RShoulderRoll': [0.00, 0.00, 0.00],
'RElbowYaw': [105.00, -15.00, 0.00],
'RElbowRoll': [0.00, 0.00, 0.00],
'RWristYaw': [55.95, 0.00, 0.00],
'LHipYawPitch': [0.00, 50.00, -85.00],
'LHipRoll': [0.00, 0.00, 0.00],
'LHipPitch': [0.00, 0.00, 0.00],
'LKneePitch': [0.00, 0.00, -100.00],
'LAnklePitch': [0.00, 0.00, -102.90],
'LAnkleRoll': [0.00, 0.00, 0.00],
'RHipYawPitch': [0.00, -50.00, -85.00],
'RHipRoll': [0.00, 0.00, 0.00],
'RHipPitch': [0.00, 0.00, 0.00],
'RKneePitch': [0.00, 0.00, -100.00],
'RAnklePitch': [0.00, 0.00, -102.90],
'RAnkleRoll': [0.00, 0.00, 0.00]
}
mX = [[1, 0, 0, 0], [0, ja_cos, -ja_sin, 0], [0, ja_sin, ja_cos, 0],
[0, 0, 0, 1]]
mY = [[ja_cos, 0, ja_sin, 0], [0, 1, 0, 0], [-ja_sin, 0, ja_cos, 0],
[0, 0, 0, 1]]
mZ = [[ja_cos, ja_sin, 0, 0], [-ja_sin, ja_cos, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]
if joint_name in [
'LShoulderRoll', 'LElbowRoll', 'LHipRoll', 'LAnkleRoll',
'RShoulderRoll', 'RElbowRoll', 'RHipRoll', 'RAnkleRoll'
]:
T = mX
elif joint_name in [
'HeadPitch', 'LShoulderPitch', 'LHipPitch', 'LKneePitch',
'LAnklePitch', 'RShoulderPitch', 'RHipPitch', 'RKneePitch',
'RAnklePitch'
]:
T = mY
elif joint_name in [
'HeadYaw', 'LElbowYaw', 'LWristYaw', 'RElbowYaw', 'RWristYaw'
]:
T = mZ
elif joint_name in ['LHipYawPitch', 'RHipYawPitch']:
T = dot(mZ, mY)
T[0][3] = mRange[joint_name][0]
T[1][3] = mRange[joint_name][1]
T[2][3] = mRange[joint_name][2]
return T
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = sin(joint_angle)
c = cos(joint_angle)
motion = { 'HeadYaw' : [0.00, 0.00, 126.50],
'HeadPitch' : [0.00, 0.00, 0.00],
'LShoulderPitch' : [0.00, 98.00, 100.00],
'LShoulderRoll' : [0.00, 0.00, 0.00],
'LElbowYaw' : [105.00, 15.00, 0.00],
'LElbowRoll' : [0.00, 0.00, 0.00],
'LWristYaw' : [55.95, 0.00, 0.00],
'RShoulderPitch' : [0.00, -98.00, 100.00],
'RShoulderRoll' : [0.00, 0.00, 0.00],
'RElbowYaw' : [105.00, -15.00, 0.00],
'RElbowRoll' : [0.00, 0.00, 0.00],
'RWristYaw' : [55.95, 0.00, 0.00],
'LHipYawPitch' : [0.00, 50.00, -85.00],
'LHipRoll' : [0.00, 0.00, 0.00],
'LHipPitch' : [0.00, 0.00, 0.00],
'LKneePitch' : [0.00, 0.00, -100.00],
'LAnklePitch' : [0.00, 0.00, -102.90],
'LAnkleRoll' : [0.00, 0.00, 0.00],
'RHipYawPitch' : [0.00, -50.00, -85.00],
'RHipRoll' : [0.00, 0.00, 0.00],
'RHipPitch' : [0.00, 0.00, 0.00],
'RKneePitch' : [0.00, 0.00, -100.00],
'RAnklePitch' : [0.00, 0.00, -102.90],
'RAnkleRoll' : [0.00, 0.00, 0.00]
}
roll = [[1,0,0,0], [0,c,-s,0], [0,s,c,0], [0,0,0,1]]
pitch = [[c,0,s,0], [0,1,0,0], [-s,0,c,0], [0,0,0,1]]
yaw = [[c,s,0,0], [-s,c,0,0], [0,0,1,0], [0,0,0,1]]
if joint_name in ['LShoulderRoll', 'LElbowRoll', 'LHipRoll', 'LAnkleRoll', 'RShoulderRoll', 'RElbowRoll', 'RHipRoll', 'RAnkleRoll']:
T = roll
elif joint_name in ['HeadPitch', 'LShoulderPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'RShoulderPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch']:
T = pitch
elif joint_name in ['HeadYaw', 'LElbowYaw', 'LWristYaw', 'RElbowYaw', 'RWristYaw']:
T = yaw
elif joint_name in ['LHipYawPitch', 'RHipYawPitch']:
T = dot(yaw, pitch)
T[0][3] = motion[joint_name][0]
T[1][3] = motion[joint_name][1]
T[2][3] = motion[joint_name][2]
return T
two_pi_f_squared_amp = np.multiply(conf.two_pi_f, two_pi_f_amp)
pEE = offset.copy()
vEE = matlib.zeros((6,1))
aEE = matlib.zeros((6,1))
tsid.gui.addSphere('world/ee', conf.SPHERE_RADIUS, conf.EE_SPHERE_COLOR)
tsid.gui.addSphere('world/ee_ref', conf.REF_SPHERE_RADIUS, conf.EE_REF_SPHERE_COLOR)
t = 0.0
q[:,0], v[:,0] = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
pEE[:3,0] = offset[:3,0] + np.multiply(conf.amp, matlib.sin(conf.two_pi_f*t + conf.phi))
vEE[:3,0] = np.multiply(two_pi_f_amp, matlib.cos(conf.two_pi_f*t + conf.phi))
aEE[:3,0] = np.multiply(two_pi_f_squared_amp, -matlib.sin(conf.two_pi_f*t + conf.phi))
sampleEE.pos(pEE)
sampleEE.vel(vEE)
sampleEE.acc(aEE)
tsid.eeTask.setReference(sampleEE)
HQPData = tsid.formulation.computeProblemData(t, q[:,i], v[:,i])
# if i == 0: HQPData.print_all()
sol = tsid.solver.solve(HQPData)
if(sol.status!=0):
print(("Time %.3f QP problem could not be solved! Error code:"%t, sol.status))
break
