def optimize_initial_position(self, init_state):
# Optimize the initial configuration
q = se3.neutral(self.robot.model)
plan_joint_init_pos = self.planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)
if len(plan_joint_init_pos) != self.robot.num_ctrl_joints:
raise ValueError(
'Number of joints in config file not same as required for robot\n'
+ 'Got %d joints but robot expects %d joints.' %
(len(plan_joint_init_pos), self.robot.num_ctrl_joints))
q[7:] = np.matrix(plan_joint_init_pos).T
q[2] = self.robot.floor_height + 0.32 #was 0.32
#print q[2]
dq = np.matrix(np.zeros(self.robot.robot.nv)).T
com_ref = init_state.com
lmom_ref = np.zeros(3)
amom_ref = np.zeros(3)
endeff_pos_ref = np.array(
[init_state.effPosition(i) for i in range(init_state.effNum())])
endeff_vel_ref = np.matrix(np.zeros((init_state.effNum(), 3)))
endeff_contact = np.ones(init_state.effNum())
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
q[3:7] = quad_goal.coeffs()
for iters in range(self.max_iterations):
# Adding small P controller for the base orientation to always start with flat
# oriented base.
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix())
res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref,
endeff_pos_ref, endeff_vel_ref,
endeff_contact, None)
q = se3.integrate(self.robot.model, q, res)
if np.linalg.norm(res) < 1e-3:
print('Found initial configuration after {} iterations'.format(
iters + 1))
break
if iters == self.max_iterations - 1:
print('Failed to converge for initial setup.')
print("initial configuration: \n", q)
q[2] = 0.20
self.q_init = q.copy()
self.dq_init = dq.copy()
def compute_contact_forces(self,
qa,
dqa,
o_R_b,
b_v_ob,
b_omg_ob,
_,
tauj,
world_frame=True):
o_quat_b = pin.Quaternion(o_R_b).coeffs()
q = np.concatenate(
[np.array([0, 0, 0]), o_quat_b,
qa]) # robot anywhere with orientation estimated from IMU
vq = np.concatenate([b_v_ob, b_omg_ob, dqa])
# vq = np.hstack([np.zeros(3), b_omg_ob, dqa])
C = pin.computeCoriolisMatrix(self.rm, self.rd, q, vq)
g = pin.computeGeneralizedGravity(self.rm, self.rd, q)
M = pin.crba(self.rm, self.rd, q)
p = M @ vq
tauj = np.concatenate([np.zeros(6), tauj])
self.int = self.int + (tauj + C.T @ vq - g + self.r) * self.dt
self.r = self.Ki * (p - self.int - self.p0)
return taucf_to_oforces(self.robot, q, self.nv, o_R_b, self.r,
self.contact_frame_id_lst)
def retrieve_pyb_data(self):
"""
Retrieve the position and orientation of the base in world frame as well as its linear and angular velocities
"""
# Joint states
jointStates = pyb.getJointStates(self.robotId, self.bullet_joint_ids)
self.qa = np.array([state[0] for state in jointStates])
self.va = np.array([state[1] for state in jointStates])
# Position and orientation of the trunk
self.w_p_b, self.w_quat_b = pyb.getBasePositionAndOrientation(
self.robotId)
self.w_p_b, self.w_quat_b = np.array(self.w_p_b), np.array(
self.w_quat_b)
self.w_R_b = pin.Quaternion(self.w_quat_b.reshape(
(4, 1))).toRotationMatrix()
# Velocity of the trunk -> world coordinates given by pyb
self.w_v_b, self.w_omg_b = pyb.getBaseVelocity(self.robotId)
self.w_v_b, self.w_omg_b = np.array(self.w_v_b), np.array(self.w_omg_b)
self.b_v_b, self.b_omg_b = self.w_R_b.T @ self.w_v_b, self.w_R_b.T @ self.w_omg_b
# Joints configuration and velocity vector for free-flyer + 12 actuators
self.q = np.hstack((self.w_p_b, self.w_quat_b, self.qa))
self.v = np.hstack((self.b_v_b, self.b_omg_b, self.va))
return 0
def log_state(self, logger, prefix):
'''Log current state: the values logged are defined in CLAPTRAP_STATE_SUFFIXES
@param logger Logger object
@param prefix Prefix to add before each suffix.
'''
rpy = se3.rpy.matrixToRpy(
se3.Quaternion(self.q[6, 0], self.q[3, 0], self.q[4, 0],
self.q[5, 0]).matrix())
logger.set(prefix + "roll", rpy[0, 0])
logger.set(prefix + "pitch", rpy[1, 0])
logger.set(prefix + "yaw", rpy[2, 0])
logger.set_vector(prefix + "omega", self.v[3:6, 0])
logger.set(
prefix + "wheelVelocity",
self.v[self.robot.model.joints[self.robot.model.
getJointId("WheelJoint")].idx_v, 0])
se3.computeAllTerms(self.robot.model, self.robot.data, self.q, self.v)
energy = self.robot.data.kinetic_energy + self.robot.data.potential_energy
logger.set(prefix + "energy", energy)
logger.set(prefix + "wheelTorque", self.tau[0, 0])
w_M_base = self.robot.framePosition(
self.q, self.robot.model.getFrameId("Body"), False)
logger.set_vector(prefix + "base", w_M_base.translation)
def pd_controller(t, q, v, q_ref, v_ref, a_ref):
pitch = se3.rpy.matrixToRpy(se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix())[1, 0]
dpitch = v[4, 0]
u = - np.matrix([[Kp * (pitch + Kd * dpitch)]])
# -u: torque is applied to the wheel, so the opposite torque is applied to the base.
return -u
def writeFromMessage(self, msg):
t = msg.time
q = np.zeros(self._model.nq)
v = np.zeros(self._model.nv)
tau = np.zeros(self._model.njoints - 2)
p = dict()
pd = dict()
f = dict()
s = dict()
# Retrieve the generalized position and velocity, and joint torques
q[3] = msg.centroidal.base_orientation.x
q[4] = msg.centroidal.base_orientation.y
q[5] = msg.centroidal.base_orientation.z
q[6] = msg.centroidal.base_orientation.w
v[3] = msg.centroidal.base_angular_velocity.x
v[4] = msg.centroidal.base_angular_velocity.y
v[5] = msg.centroidal.base_angular_velocity.z
for j in range(len(msg.joints)):
jointId = self._model.getJointId(msg.joints[j].name) - 2
q[jointId + 7] = msg.joints[j].position
v[jointId + 6] = msg.joints[j].velocity
tau[jointId] = msg.joints[j].effort
pinocchio.centerOfMass(self._model, self._data, q, v)
q[0] = msg.centroidal.com_position.x - self._data.com[0][0]
q[1] = msg.centroidal.com_position.y - self._data.com[0][1]
q[2] = msg.centroidal.com_position.z - self._data.com[0][2]
v[0] = msg.centroidal.com_velocity.x - self._data.vcom[0][0]
v[1] = msg.centroidal.com_velocity.y - self._data.vcom[0][1]
v[2] = msg.centroidal.com_velocity.z - self._data.vcom[0][2]
# Retrive the contact information
for contact in msg.contacts:
name = contact.name
# Contact pose
pose = contact.pose
position = np.array(
[pose.position.x, pose.position.y, pose.position.z])
quaternion = pinocchio.Quaternion(pose.orientation.w,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z)
p[name] = pinocchio.SE3(quaternion, position)
# Contact velocity
velocity = contact.velocity
lin_vel = np.array(
[velocity.linear.x, velocity.linear.y, velocity.linear.z])
ang_vel = np.array(
[velocity.angular.x, velocity.angular.y, velocity.angular.z])
pd[name] = pinocchio.Motion(lin_vel, ang_vel)
# Contact wrench
wrench = contact.wrench
force = np.array([wrench.force.x, wrench.force.y, wrench.force.z])
torque = np.array(
[wrench.torque.x, wrench.torque.y, wrench.torque.z])
f[name] = [contact.type, pinocchio.Force(force, torque)]
# Surface normal and friction coefficient
normal = contact.surface_normal
nsurf = np.array([normal.x, normal.y, normal.z])
s[name] = [nsurf, contact.friction_coefficient]
return t, q, v, tau, p, pd, f, s
def transform_msg(transforms, Tmean, Tvar):
return """Average transform from {} transforms:
- translation: {}
- rotation (x, y, z, w): {}
- variance: {}""".format(len(transforms),
Tmean.translation,
pinocchio.Quaternion(Tmean.rotation).coeffs(),
Tvar)
def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel):
"""Compute the desired COM wrench (equation 1).
Args:
des_pos: desired center of mass position at time t
des_vel: desired center of mass velocity at time t
des_ori: desired base orientation at time t (quaternions)
des_angvel: desired base angular velocity at time t
Returns:
Computed w_com
"""
m = self.robot_mass
robot = self.robot
# com = np.reshape(np.array(q[0:3]), (3,))
com = pin.centerOfMass(robot.pin_robot.model, robot.pin_robot.data, q,
dq)
vcom = np.reshape(np.array(dq[0:3]), (3, ))
Ib = robot.pin_robot.mass(q)[3:6, 3:6]
quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori))
cur_angvel = arr(dq[3:6])
if self.rotate_vel_error:
# Rotate the des and current angular velocity into the world frame.
quat_des = pin.Quaternion(des_ori[3], des_ori[0], des_ori[1],
des_ori[2]).matrix()
des_angvel = quat_des.dot(des_angvel)
quat_cur = pin.Quaternion(q[6], q[3], q[4], q[5]).matrix()
cur_angvel = quat_cur.dot(cur_angvel)
w_com = np.hstack([
m * np.multiply(self.kc, des_pos - com) +
m * np.multiply(self.dc, des_vel - vcom),
arr(
arr(np.multiply(self.kb, quat_diff)) +
(Ib * mat(np.multiply(self.db, des_angvel - cur_angvel))).T)
])
# adding weight
# w_com[2] += m*9.81
return w_com
def integrate_dv_R3SO3(self, q, v, dv, dt):
b_v = v[0:3]
b_w = v[3:6]
b_acc = dv[0:3] + np.cross(b_w, b_v)
p_int = q[:3]
oRb_int = pin.Quaternion(q[3:7].reshape((4, 1))).toRotationMatrix()
v_int = oRb_int @ v[:3]
p_int = p_int + v_int * dt + 0.5 * oRb_int @ b_acc * dt**2
v_int = v_int + oRb_int @ b_acc * dt
oRb_int = oRb_int @ pin.exp(b_w * dt)
q[:3] = p_int
q[3:7] = pin.Quaternion(oRb_int).coeffs()
q[7:] += v[6:] * dt
v += dt * dv
v[:3] = oRb_int.T @ v_int
return q, v
def SphericalToRPY(joint):
# i.e. SphericalToRPY('hip_r')
i = pinocchioRobot.getDoFIdx(joint)
quat = np.matrix([
pinocchioRobot.q[i, 0], pinocchioRobot.q[i + 1, 0],
pinocchioRobot.q[i + 2, 0], pinocchioRobot.q[i + 3, 0]
], np.float)
quat = np.squeeze(np.asarray(quat))
rpy = se3.utils.matrixToRpy(
se3.Quaternion(quat[3], quat[0], quat[1], quat[2]).matrix())
return rpy
def objects_pos_orn_rand_falling(self):
self.show_plane()
dheight = 0.05
for n, body in enumerate(self.bodies):
pos = self.np_random.uniform(*self.objects_xyz_interval)
pos[2] = dheight * (n + 1)
orn = pin.Quaternion(pin.SE3.Random().rotation).coeffs()
body.pose = pos, orn
ms = self.np_random.randint(5, 10) * 100
self.run_simulation(float(ms) * 1e-3)
def get_current_config(robot, graph, q0, timeout=5.):
init_ros_node()
import tf2_ros, rospy, pinocchio
from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import JointState
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
# Build initial position
msg = rospy.wait_for_message("/amcl_pose", PoseWithCovarianceStamped)
#P = msg.pose.pose.position
#Q = msg.pose.pose.orientation
mMb = poseToSE3(msg.pose.pose)
try:
_wMm = tfBuffer.lookup_transform("world", msg.header.frame_id,
msg.header.stamp,
rospy.Duration(timeout))
wMm = poseToSE3(_wMm.transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print('could not get TF transform : ', e)
return
wMb = wMm * mMb
# conversion from
# cos(t) = 2 cos(t/2)**2 - 1
# sin(t) = 2 cos(t/2) sin(t/2)
P = wMb.translation
Q = pinocchio.Quaternion(wMb.rotation)
q = q0[:]
q[0:4] = P[0], P[1], 2 * Q.w**2 - 1, 2 * Q.w * Q.z
# Acquire robot state
msg = rospy.wait_for_message("/joint_states", JointState)
for ni, qi in zip(msg.name, msg.position):
jni = "tiago/" + ni
if robot.getJointConfigSize(jni) != 1:
continue
try:
rk = robot.rankInConfiguration[jni]
except KeyError:
continue
assert robot.getJointConfigSize(jni) == 1
q[rk] = qi
# put driller in hand
res, q0proj, err = graph.applyNodeConstraints(
"tiago/gripper grasps driller/handle", q)
if not res:
print(
"Could not project onto state 'tiago/gripper grasps driller/handle'",
err)
return q0proj
def dynamics(self, t, x):
''' Forward dynamics of the robot, to integrate
'''
# Split input as (q, v) pair
q = np.matrix(x[:self.robot.nq]).T
v = np.matrix(x[self.robot.nq:]).T
# Renormalize quaternion to prevent propagation of rounding errors due to integration.
q[3:7, 0] /= np.linalg.norm(q[3:7, 0])
# Run forward dynamic computation
# Compute H and g + coloriolis effects
H = self.robot.mass(q)
g = self.robot.nle(q, v)
# Compute jacobian
# Note that the jacobian only depends on data directly available in q, v, so there is no need for advanced
# computation
radius_world = se3.Quaternion(q[6, 0], q[3, 0], q[4, 0],
q[5, 0]).conjugate() * self.wheel_radius
omega = v[3:6, 0]
skew_radius = skew_symmetric(radius_world)
omega_skew_radius = skew_symmetric(-skew_symmetric(omega) *
radius_world)
J = np.concatenate(
(np.eye(3), skew_radius, skew_radius * np.matrix([[0, 1, 0]]).T),
axis=1)
dJ = np.concatenate((np.zeros((3, 3)), omega_skew_radius,
omega_skew_radius * np.matrix([[0, 1, 0]]).T),
axis=1)
drift = -dJ * v
# Compute controller torque
torque = np.zeros((self.robot.model.nv, 1))
# Consider all joints actuated except freeflyer
torque[6:, 0] = self.controller(t, q, v, None, None, None)
# Write full equation
A = np.block([[H, J.T], [J, np.zeros((3, 3))]])
# ~ b = np.concatenate((torque - g, drift))
b = np.concatenate((torque - g, drift))
# Find dv
dv = np.linalg.solve(A, b)[:-3]
# Now we just need to integrate dv
# The slight issue is that q is not a term-by-term integral of v (because of the freeflyer).
# Here we use pinocchio's integrate funciton to compute the numerical derivative of q
dt = 1e-6
dq = ((se3.integrate(self.robot.model, q, dt * v) - q) / dt)
return np.concatenate((dq, dv))
def read_data_file_laas(file_path, dt):
# Load data file
data = np.load(file_path)
# o_a_oi: litteraly the imu ref frame origin acceleration (gravity is compensated!)
arr_dic = {
'imu_acc': data['baseAccelerometer'],
'o_a_oi': data['baseLinearAcceleration'],
'o_q_i': data['baseOrientation'],
'i_omg_oi': data['baseAngularVelocity'],
'qa': data['q_mes'],
'dqa': data['v_mes'],
'tau': data['torquesFromCurrentMeasurment'],
'w_p_wm': data['mocapPosition'],
'w_q_m': data['mocapOrientationQuat'],
'w_R_m': data['mocapOrientationMat9'],
'w_v_wm': data['mocapVelocity']
}
N = min(arr.shape[0] for key, arr in arr_dic.items())
arr_dic = {key: arr[:N, :] for key, arr in arr_dic.items()}
# shortest time array fitting all traj
t_arr = np.arange(N) * dt # in seconds, starting from 0
arr_dic['t'] = t_arr
# !!!!! Mocap orientation needs to be inverted !!!!!!
# quat -> conjugate
# R mat -> transpose
# arr_dic['w_q_m'][:,:3] = -arr_dic['w_q_m'][:,:3] # qx, qy, qz, qw -> -qx, -qy, -qz, qw
# arr_dic['w_R_m'] = np.array([w_R_m.T for w_R_m in arr_dic['w_R_m']])
# other preprocessing (to unify formats)
arr_dic['w_pose_wm'] = np.hstack([arr_dic['w_p_wm'], arr_dic['w_q_m']])
arr_dic['m_v_wm'] = np.array([
w_R_m.T @ w_v_wm
for w_R_m, w_v_wm in zip(arr_dic['w_R_m'], arr_dic['w_v_wm'])
]) # compute local mocap base velocity
arr_dic['o_R_i'] = np.array([
pin.Quaternion(o_q_i.reshape(4, 1)).toRotationMatrix()
for o_q_i in arr_dic['o_q_i']
])
# roll/pitch/yaw
arr_dic['o_rpy_i'] = np.array(
[pin.rpy.matrixToRpy(R) for R in arr_dic['o_R_i']])
arr_dic['w_rpy_m'] = np.array(
[pin.rpy.matrixToRpy(R) for R in arr_dic['w_R_m']])
if 'contactStatus' in data:
arr_dic['contactStatus'] = data['contactStatus']
return arr_dic
def test_matrixToRpy(self):
n = 100
for _ in range(n):
quat = pin.Quaternion(np.random.rand(4, 1)).normalized()
R = quat.toRotationMatrix()
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
n2 = 100
# Test singular case theta = pi/2
Rp = np.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]])
for _ in range(n2):
r = random() * 2 * pi - pi
y = random() * 2 * pi - pi
R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(Rp) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
# Test singular case theta = -pi/2
Rp = np.array([[0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]])
for _ in range(n2):
r = random() * 2 * pi - pi
y = random() * 2 * pi - pi
R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(Rp) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
def compute_contact_forces(self,
qa,
dqa,
o_R_b,
b_v_ob,
b_omg_ob,
o_a_ob,
tauj,
world_frame=True):
"""
Compute the 3D contact/reaction forces at the solo feet using inverse dynamics.
Forces are expressed in body frame.
"""
b_v_ob = np.zeros(3)
o_quat_b = pin.Quaternion(o_R_b).coeffs()
q = np.concatenate(
[np.array([0, 0, 0]), o_quat_b,
qa]) # robot anywhere with orientation estimated from IMU
vq = np.concatenate(
[b_v_ob, b_omg_ob,
dqa]) # generalized velocity in base frame as pin requires
# vq = np.hstack([np.zeros(3), b_omg_ob, dqa]) # generalized velocity in base frame as pin requires
b_spa = o_R_b.T @ o_a_ob - np.cross(
b_omg_ob,
b_v_ob) # spatial acceleration linear part, neglecting the rest
ddqa = (
dqa - self.dqa_prev
) / self.dt # using ddqa numdiff only seems to yield the best results
# ddqa = np.zeros(12)
# b_domgdt_ob = (b_omg_ob - self.b_omg_ob_prev)/self.dt
b_domgdt_ob = np.zeros(3)
dvq = np.concatenate([b_spa, b_domgdt_ob, ddqa])
taucf = pin.rnea(
self.robot.model, self.robot.data, q, vq, dvq
) # sum of joint torque + contact torques corresponding to the actuated equations
taucf[6:] = taucf[6:] - tauj
self.dqa_prev = dqa
self.b_omg_ob_prev = b_omg_ob
return taucf_to_oforces(self.robot,
q,
self.nv,
o_R_b,
taucf,
self.contact_frame_id_lst,
world_frame=world_frame)
def UpdateMeasurment(self):
"""Retrieve data about the robot from the simulation to mimic what the masterboard does
"""
# Position and velocity of actuators
jointStates = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints
self.q_mes[:] = np.array([state[0] for state in jointStates])
self.v_mes[:] = np.array([state[1] for state in jointStates])
# Measured torques
self.torquesFromCurrentMeasurment[:] = self.jointTorques[:].ravel()
# Position and orientation of the trunk (PyBullet world frame)
self.baseState = pyb.getBasePositionAndOrientation(self.pyb_sim.robotId)
self.dummyHeight = np.array(self.baseState[0])
self.dummyHeight[2] = 0.20
self.dummyPos = np.array(self.baseState[0])
# Linear and angular velocity of the trunk (PyBullet world frame)
self.baseVel = pyb.getBaseVelocity(self.pyb_sim.robotId)
# print("baseVel: ", np.array([self.baseVel[0]]))
# Orientation of the base (quaternion)
self.baseOrientation[:] = np.array(self.baseState[1])
RPY = quaternionToRPY(self.baseOrientation)
self.hardware.roll = RPY[0]
self.hardware.pitch = RPY[1]
self.hardware.yaw = RPY[2]
self.rot_oMb = pin.Quaternion(np.array([self.baseOrientation]).transpose()).toRotationMatrix()
self.oMb = pin.SE3(self.rot_oMb, np.array([self.dummyHeight]).transpose())
# Angular velocities of the base
self.baseAngularVelocity[:] = (self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose()).ravel()
# Linear Acceleration of the base
self.o_baseVel = np.array([self.baseVel[0]]).transpose()
self.b_baseVel = (self.oMb.rotation.transpose() @ self.o_baseVel).ravel()
self.o_imuVel = self.o_baseVel + self.oMb.rotation @ self.cross3(np.array([0.1163, 0.0, 0.02]), self.baseAngularVelocity[:])
self.baseLinearAcceleration[:] = (self.oMb.rotation.transpose() @ (self.o_imuVel - self.prev_o_imuVel)).ravel() / self.dt
self.prev_o_imuVel[:, 0:1] = self.o_imuVel
self.baseAccelerometer[:] = self.baseLinearAcceleration[:] + \
(self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])).ravel()
return
def velocityXYZQuatToXYZRPY(xyzquat: np.ndarray,
v: np.ndarray) -> np.ndarray:
"""Convert the derivative of [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw].
.. note:
No need to estimate yaw angle to get RPY velocity, since
`computeRpyJacobianInverse` only depends on Roll and Pitch angles.
However, it is not the case for the linear velocity.
.. warning::
Linear velocity in XYZRPY must be local-world-aligned frame, while
returned linear velocity in XYZQuat is in local frame.
"""
quat = pin.Quaternion(xyzquat[3:])
rpy = matrixToRpy(quat.matrix())
J_rpy_inv = computeRpyJacobianInverse(rpy)
return np.concatenate((quat * v[:3], J_rpy_inv @ v[3:]))
def __init__(self, *args):
if len(args) == 0:
raise NotImplementedError
elif len(args) == 7:
raise NotImplementedError
elif len(args) == 1:
arg = args[0]
if isinstance(arg, Transform):
self.T = arg.T
elif isinstance(arg, pin.SE3):
self.T = arg
else:
arg_array = np.asarray(arg)
if arg_array.shape == (4, 4):
R = arg_array[:3, :3]
t = arg_array[:3, -1]
self.T = pin.SE3(R, t.reshape(3, 1))
else:
raise NotImplementedError
elif len(args) == 2:
args_0_array = np.asarray(args[0])
n_elem_rot = len(args_0_array.flatten())
if n_elem_rot == 4:
xyzw = np.asarray(args[0]).flatten().tolist()
wxyz = [xyzw[-1], *xyzw[:-1]]
assert len(wxyz) == 4
q = pin.Quaternion(*wxyz)
q.normalize()
R = q.matrix()
elif n_elem_rot == 9:
assert args_0_array.shape == (3, 3)
R = args_0_array
t = np.asarray(args[1])
assert len(t) == 3
self.T = pin.SE3(R, t.reshape(3, 1))
elif len(args) == 1:
if isinstance(args[0], Transform):
raise NotImplementedError
elif isinstance(args[0], list):
array = np.array(args[0])
assert array.shape == (4, 4)
self.T = pin.SE3(array)
elif isinstance(args[0], np.ndarray):
assert args[0].shape == (4, 4)
self.T = pin.SE3(args[0])
def compute_contact_forces2(self,
qa,
dqa,
ddqa,
o_R_i,
i_omg_oi,
i_domg_oi,
o_a_oi,
tauj,
world_frame=True):
"""
Compute the 3D contact/reaction forces at the solo feet using inverse dynamics.
Forces are expressed in body frame.
"""
zero3 = np.array([0, 0, 0])
o_quat_i = pin.Quaternion(o_R_i).coeffs()
q = np.concatenate(
[zero3, o_quat_i,
qa]) # robot anywhere with orientation estimated from IMU
vq = np.concatenate(
[zero3, i_omg_oi,
dqa]) # generalized velocity in base frame as pin requires
i_a_oi = o_R_i.T @ o_a_oi # - np.cross(b_omg_ob, b_v_ob) # since the i_v_oi is set to zero arbitrarily
dvq = np.concatenate([i_a_oi, i_domg_oi, ddqa])
taucf = pin.rnea(
self.robot.model, self.robot.data, q, vq, dvq
) # sum of joint torque + contact torques corresponding to the actuated equations
taucf[6:] = taucf[6:] - tauj
self.dqa_prev = dqa
self.i_omg_oi_prev = i_omg_oi
return taucf_to_oforces(self.robot,
q,
self.nv,
o_R_i,
taucf,
self.contact_frame_id_lst,
world_frame=world_frame)
def check_pyb_env(self, qmes12):
"""Check the state of the robot to trigger events and update camera
Args:
qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
"""
# Check if the robot is in front of the first sphere to trigger it
if self.flag_sphere1 and (qmes12[1, 0] >= 0.9):
pyb.resetBaseVelocity(self.sphereId1,
linearVelocity=[3.0, 0.0, 2.0])
self.flag_sphere1 = False
# Check if the robot is in front of the second sphere to trigger it
if self.flag_sphere2 and (qmes12[1, 0] >= 1.1):
pyb.resetBaseVelocity(self.sphereId2,
linearVelocity=[-3.0, 0.0, 2.0])
self.flag_sphere2 = False
# Apply perturbation
# pyb.applyExternalForce(pyb_sim.robotId, -1, [1.0, 0.0, 0.0], [0.0, 0.0, 0.0], pyb.LINK_FRAME)"""
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
"""pyb.resetDebugVisualizerCamera(cameraDistance=0.75, cameraYaw=+50, cameraPitch=-35,
cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])"""
oMb_tmp = pin.SE3(pin.Quaternion(qmes12[3:7]),
np.array([0.0, 0.0, 0.0]))
RPY = pin.rpy.matrixToRpy(oMb_tmp.rotation)
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
pyb.resetDebugVisualizerCamera(
cameraDistance=0.6,
cameraYaw=(RPY[2, 0] * (180 / 3.1415) + 45),
cameraPitch=-39.9,
cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])
return 0
def UpdateMeasurment(self):
# Position and velocity of actuators
jointStates = pyb.getJointStates(
self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints
self.q_mes[:] = np.array([state[0] for state in jointStates])
self.v_mes[:] = np.array([state[1] for state in jointStates])
# Measured torques
self.torquesFromCurrentMeasurment[:] = self.jointTorques[:].ravel()
# Position and orientation of the trunk (PyBullet world frame)
self.baseState = pyb.getBasePositionAndOrientation(
self.pyb_sim.robotId)
self.dummyHeight = np.array(self.baseState[0])
self.dummyHeight[2] = 0.20
# Linear and angular velocity of the trunk (PyBullet world frame)
self.baseVel = pyb.getBaseVelocity(self.pyb_sim.robotId)
# Orientation of the base (quaternion)
self.baseOrientation[:] = np.array(self.baseState[1])
self.rot_oMb = pin.Quaternion(
np.array([self.baseState[1]]).transpose()).toRotationMatrix()
self.oMb = pin.SE3(self.rot_oMb, np.array(
[self.dummyHeight]).transpose())
# Angular velocities of the base
self.baseAngularVelocity[:] = (self.oMb.rotation.transpose() @ np.array([
self.baseVel[1]]).transpose()).ravel()
# Linear Acceleration of the base
self.b_baseVel = (self.oMb.rotation.transpose() @ np.array(
[self.baseVel[0]]).transpose()).ravel()
self.baseLinearAcceleration[:] = (
self.b_baseVel.ravel() - self.prev_b_baseVel) / self.dt
self.prev_b_baseVel[:] = self.b_baseVel.ravel()
return
def extract_new_positions(self):
# This function needs to return:
# 1: end_effector position
# 2: end_effector velocity
# 3: end_effector orientation
current_c_pos = np.array(
self.data.oMf[self.end_effector_frame_id].translation)
current_c_pos += self.offset
# should return 0 always as we do not give torques
# current_c_vel = np.array(pinocchio.getFrameVelocity(self.model, self.data,
# self.model.getFrameId('panda_grasptarget')))
current_c_vel = np.zeros(3)
# current_c_vel = (current_c_pos - self.c_pos_old)*self.dt
rotation_matrix = np.array(
self.data.oMf[self.end_effector_frame_id].rotation)
quat_pin = pinocchio.Quaternion(self.data.oMf[
self.end_effector_frame_id].rotation).coeffs() # [ x, y, z, w]
current_c_quat = np.zeros(4)
current_c_quat[1:] = quat_pin[:3]
current_c_quat[0] = quat_pin[-1]
return current_c_pos, current_c_vel, current_c_quat
for i in range(N):
M = se3.exp(nu) * M
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
Mc = se3.SE3(M.rotation, zero(3))
M = M * se3.exp(Mc.actInv(nu))
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a constant "log" velocity in body frame.
ME = se3.SE3(
se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
np.matrix([1, -1, 2], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)
# Integrate a constant "log" velocity in reference frame.
ME = se3.SE3(
se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
np.matrix([-1, 1, 0.6], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
M = M * se3.exp(nu)
ax[j].set_ylim([
np.min(x[j, :]) - 0.1 * (X_MAX[j] - X_MIN[j]),
np.max(x[j, :]) + 0.1 * (X_MAX[j] - X_MIN[j])
])
ax[6].set_xlabel(xlabel)
ax[7].set_xlabel(xlabel)
ax[0].set_title(name)
#plut.saveFigure(TEST_NAME+'_'+name);
plut.saveFigureandParameterinDateFolder(GARBAGE_FOLDER,
TEST_NAME + '_' + name, PARAMS)
return ax
# Convertion from translation+quaternion to SE3 and reciprocally
q2m = lambda q: se3.SE3(
se3.Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).array(), q[:3]
) # matrix
m2q = lambda M: np.vstack([M.translation, se3.Quaternion(M.rotation).coeffs()])
''' PLOT-RELATED USER PARAMETERS '''
LW = 4
# line width
LINE_ALPHA = 1.0
LEGEND_ALPHA = 1.0
line_styles = ["c-", "b--", "g-.", "k:", "m-"]
PLOT_JOINT_POS_VEL_ACC = True
PLOT_STATE_SPACE = 1
Q_INTERVAL = 0.001
# the range of possible angles is sampled with this step for plotting
TEST_STANDARD = 1
TEST_VIABILITY = 0
TEST_RANDOM = 0
def XYZQuatToXYZRPY(xyzquat):
"""Convert [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw].
"""
return np.concatenate((
xyzquat[:3], matrixToRpy(pin.Quaternion(xyzquat[3:]).matrix())))
if RUN_SIMULATION:
t1 = time.time()
try:
subprocess.run(RUN_FILE, stdout=subprocess.DEVNULL, check=True)
except subprocess.CalledProcessError as e:
raise (e)
print('time: ', time.time() - t1)
df_gtr = pd.read_csv('gtr.csv')
df_est = pd.read_csv('est.csv')
df_fbk = pd.read_csv('fbk.csv')
df_kfs = pd.read_csv('kfs.csv')
# compute angle axis
quat_gtr_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_gtr['qx'], df_gtr['qy'], df_gtr['qz'], df_gtr['qw'])
]
quat_est_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_est['qx'], df_est['qy'], df_est['qz'], df_est['qw'])
]
quat_fbk_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_fbk['qx'], df_fbk['qy'], df_fbk['qz'], df_fbk['qw'])
]
quat_kfs_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_kfs['qx'], df_kfs['qy'], df_kfs['qz'], df_kfs['qw'])
]
import os
import sys
import crocoddyl
import pinocchio
import numpy as np
import example_robot_data
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
hector = example_robot_data.load('hector')
robot_model = hector.model
target_pos = np.array([1., 0., 1.])
target_quat = pinocchio.Quaternion(1., 0., 0., 0.)
state = crocoddyl.StateMultibody(robot_model)
d_cog, cf, cm, u_lim, l_lim = 0.1525, 6.6e-5, 1e-6, 5., 0.1
tau_f = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [1., 1., 1., 1.], [0., d_cog, 0., -d_cog],
[-d_cog, 0., d_cog, 0.], [-cm / cf, cm / cf, -cm / cf, cm / cf]])
actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f)
nu = actuation.nu
runningCostModel = crocoddyl.CostModelSum(state, nu)
terminalCostModel = crocoddyl.CostModelSum(state, nu)
# Costs
xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(np.array([0.1] * 3 + [1000.] * 3 + [1000.] * robot_model.nv))
import os
import sys
import crocoddyl
import pinocchio
import numpy as np
import example_robot_data
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
WITHDISPLAY = True
hector = example_robot_data.loadHector()
robot_model = hector.model
target_pos = np.array([1, 0, 1])
target_quat = pinocchio.Quaternion(1, 0, 0, 0)
state = crocoddyl.StateMultibody(robot_model)
d_cog = 0.1525
cf = 6.6e-5
cm = 1e-6
u_lim = 5
l_lim = 0.1
tau_f = np.array([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [1.0, 1.0, 1.0, 1.0], [0.0, d_cog, 0.0, -d_cog],
[-d_cog, 0.0, d_cog, 0.0], [-cm / cf, cm / cf, -cm / cf, cm / cf]])
actModel = crocoddyl.ActuationModelMultiCopterBase(state, 4, tau_f)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
def logarithmicMap(self, quat_wxyz):
w, x, y, z = quat_wxyz
return pin.log(pin.Quaternion(w, x, y, z).matrix())
