def fk(self): """ :param model: model :type model: rbdl.model :return: 2D array of the joint locations """ x = [] y = [] point_local = np.array([0.0, 0.0, 0.0]) data = rbdl.CalcBodyToBaseCoordinates(self._model, self.q, self.b0, point_local) x.append(data[0]) y.append(data[2]) data = rbdl.CalcBodyToBaseCoordinates(self._model, self.q, self.b1, point_local) x.append(data[0]) y.append(data[2]) data = rbdl.CalcBodyToBaseCoordinates(self._model, self.q, self.b2, point_local) x.append(data[0]) y.append(data[2]) return (x, y)
def test_UpdateKinematicsConsistency(self): contact_body_id = self.body_1 contact_point = np.array([0., -1., 0.]) self.q[0] = 0.1 self.q[1] = 0.2 self.q[2] = 0.3 self.q[3] = 0.4 self.q[4] = 0.5 self.q[5] = 0.6 rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot) point1 = rbdl.CalcBodyToBaseCoordinates(self.model, self.q, contact_body_id, contact_point) rbdl.UpdateKinematicsCustom(self.model, self.q) point2 = rbdl.CalcBodyToBaseCoordinates(self.model, self.q, contact_body_id, contact_point) self.qdot[0] = 1.1 self.qdot[1] = 1.2 self.qdot[2] = 1.3 self.qdot[3] = -1.4 self.qdot[4] = -1.5 self.qdot[5] = -1.6 rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot) point_velocity1 = rbdl.CalcPointVelocity(self.model, self.q, self.qdot, contact_body_id, contact_point) rbdl.UpdateKinematicsCustom(self.model, self.q, self.qdot) point_velocity2 = rbdl.CalcPointVelocity(self.model, self.q, self.qdot, contact_body_id, contact_point) self.qdot[0] = 10.1 self.qdot[1] = 10.2 self.qdot[2] = 10.3 self.qdot[3] = -10.4 self.qdot[4] = -10.5 self.qdot[5] = -10.6 rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot) point_acceleration1 = rbdl.CalcPointAcceleration( self.model, self.q, self.qdot, self.qddot, contact_body_id, contact_point) rbdl.UpdateKinematicsCustom(self.model, self.q, self.qdot, self.qddot) point_acceleration2 = rbdl.CalcPointAcceleration( self.model, self.q, self.qdot, self.qddot, contact_body_id, contact_point) assert_almost_equal(point1, point2) assert_almost_equal(point_velocity1, point_velocity2) assert_almost_equal(point_acceleration1, point_acceleration2)
def run(data): q = data # print(q) j0_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1, np.array([0., l1, 0.])) j1_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1, np.array([0., 0., 0.])) j2_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3, np.array([0., 0., 0.])) j3_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3, np.array([0., l3, 0.])) link1.set_data([j0_p[1], j1_p[1]], [j0_p[2], j1_p[2]]) link2.set_data([j1_p[1], j2_p[1]], [j1_p[2], j2_p[2]]) link3.set_data([j2_p[1], j3_p[1]], [j2_p[2], j3_p[2]])
def run(data): show_axis = 'yz' q, fc = data pA = rbdl.CalcBodyToBaseCoordinates(model, q, id_torso, np.array([0., 0., 0.2])) pB = rbdl.CalcBodyToBaseCoordinates(model, q, id_torso, np.array([0., 0., -0.2])) pC = rbdl.CalcBodyToBaseCoordinates(model, q, idl_link, np.array([0., 0., 0.])) pD = rbdl.CalcBodyToBaseCoordinates(model, q, idl_thigh, np.array([0., 0., -0.5])) pE = rbdl.CalcBodyToBaseCoordinates(model, q, idl_shank, np.array([0., 0., -0.5])) pF = rbdl.CalcBodyToBaseCoordinates(model, q, idr_link, np.array([0., 0., 0.])) pG = rbdl.CalcBodyToBaseCoordinates(model, q, idr_thigh, np.array([0., 0., -0.5])) pH = rbdl.CalcBodyToBaseCoordinates(model, q, idr_shank, np.array([0., 0., -0.5])) if show_axis=='yz': AB.set_data([pA[1], pB[1]], [pA[2], pB[2]]) FC.set_data([pF[1], pC[1]], [pF[2], pC[2]]) CD.set_data([pC[1], pD[1]], [pC[2], pD[2]]) DE.set_data([pD[1], pE[1]], [pD[2], pE[2]]) FG.set_data([pF[1], pG[1]], [pF[2], pG[2]]) GH.set_data([pG[1], pH[1]], [pG[2], pH[2]]) else: AB.set_data([pA[0], pB[0]], [pA[2], pB[2]]) FC.set_data([pF[0], pC[0]], [pF[2], pC[2]]) CD.set_data([pC[0], pD[0]], [pC[2], pD[2]]) DE.set_data([pD[0], pE[0]], [pD[2], pE[2]]) FG.set_data([pF[0], pG[0]], [pF[2], pG[2]]) GH.set_data([pG[0], pH[0]], [pG[2], pH[2]])
def run(data): q = data j0_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1, np.array([0., l1, 0.])) j1_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link1, np.array([0., 0., 0.])) j2_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3, np.array([0., 0., 0.])) j3_p = rbdl.CalcBodyToBaseCoordinates(model, q, b_link3, np.array([0., l3, 0.])) ax.plot([j0_p[1], j1_p[1]], [j0_p[2], j1_p[2]], 'r') ax.plot([j1_p[1], j2_p[1]], [j1_p[2], j2_p[2]], 'b') ax.plot([j2_p[1], j3_p[1]], [j2_p[2], j3_p[2]], 'g')
def __init__(self, model, q, qd, body_id, body_point_position, update_kinematics=True): self.body_id = body_id self.body_point_position = body_point_position self.pos = rbdl.CalcBodyToBaseCoordinates(model, q, body_id, body_point_position, update_kinematics) self.rot = model.X_base[body_id].E.T self.transform = homogeneous_matrix(rot=self.rot, pos=self.pos) self.quaternion = tf_transformations.quaternion_from_matrix( homogeneous_matrix(rot=self.rot) ) self.rpy = np.array(tf_transformations.euler_from_matrix( homogeneous_matrix(rot=self.rot), axes='sxyz')) self.pose = np.concatenate((self.rpy, self.pos)) self.vel = rbdl.CalcPointVelocity6D(model, q, qd, body_id, body_point_position, update_kinematics) self.angular_vel = self.vel[:3] self.linear_vel = self.vel[-3:] self.Jdqd = rbdl.CalcPointAcceleration6D( model, q, qd, np.zeros(model.dof_count), body_id, body_point_position, update_kinematics ) self.temp = np.zeros((6, model.dof_count + 1)) # this is a bug rbdl.CalcPointJacobian6D(model, q, body_id, body_point_position, self.temp, update_kinematics) self.J = self.temp[:6, :model.dof_count]
def test_InverseKinematics (self): """ Checks whether inverse kinematics methods are consistent """ q = np.random.rand (self.model.q_size) q_res = np.random.rand (self.model.q_size) qCS_res = np.random.rand (self.model.q_size) target_body_ids = np.array([self.body_3]) body_points = np.array([np.zeros(3)]) body_points[0][2] = 1.0 target_positions = np.array([np.zeros(3)]) target_positions[0][2] = 1.0 target_positions[0][0] = 2.0 ori_matrix = np.array([[0., 0., -1.], [0., 1., 0.], [1., 0., 0.]]) CS = rbdl.InverseKinematicsConstraintSet() CS_size = CS.AddPointConstraint (target_body_ids[0], body_points[0], target_positions[0] ) CS.AddOrientationConstraint (self.body_1, ori_matrix) CS.dlambda = 0.9 CS.max_steps = 2000 CS.step_tol = 1.0e-8 rbdl.InverseKinematics ( self.model, q, target_body_ids, body_points, target_positions, q_res, CS.step_tol, CS.dlambda, CS.max_steps ) rbdl.InverseKinematicsCS ( self.model, q, CS, qCS_res ) res_point = rbdl.CalcBodyToBaseCoordinates(self.model, q_res, self.body_3, body_points[0]) res_point2 = rbdl.CalcBodyToBaseCoordinates(self.model, qCS_res, self.body_3, body_points[0]) res_ori = rbdl.CalcBodyWorldOrientation (self.model, qCS_res, self.body_1) assert_almost_equal (target_positions[0], res_point) assert_almost_equal (target_positions[0], res_point2) assert_almost_equal (ori_matrix, res_ori)
def fk(self): fk = {} point_local = np.array([0.0, 0.0, 0.0]) data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q, self.left_thigh, point_local) fk["left_hip"] = Point.Point(data[0], data[1], data[2]) data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q, self.left_shank, point_local) fk["left_knee"] = Point.Point(data[0], data[1], data[2]) data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q, self.left_foot, point_local) fk["left_ankle"] = Point.Point(data[0], data[1], data[2]) data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q, self.right_thigh, point_local) fk["right_hip"] = Point.Point(data[0], data[1], data[2]) data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q, self.right_shank, point_local) fk["right_knee"] = Point.Point(data[0], data[1], data[2]) data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q, self.right_foot, point_local) fk["right_ankle"] = Point.Point(data[0], data[1], data[2]) q_left = self.get_left_leg().ankle.angle.z q_right = self.get_right_leg().ankle.angle.z fk["left_toe"] = Point.Point(0, 0, 0) fk["left_toe"].x = fk["left_ankle"].x - 0.8 * ( 8.0 / 100.0) * self._height * np.cos(q_left) fk["left_toe"].y = fk["left_ankle"].y - 0.8 * ( 8.0 / 100.0) * self._height * np.cos(q_left) fk["left_toe"].z = fk["left_ankle"].z - 0.05 + 0.8 * ( 8.0 / 100.0) * self._height * np.sin(q_left) fk["left_heel"] = Point.Point(0, 0, 0) fk["left_heel"].x = fk["left_ankle"].x + 0.2 * ( 8.0 / 100.0) * self._height * np.cos(q_left) fk["left_heel"].y = fk["left_ankle"].y + 0.2 * ( 8.0 / 100.0) * self._height * np.cos(q_left) fk["left_heel"].z = fk["left_ankle"].z - 0.05 + 0.2 * ( 8.0 / 100.0) * self._height * np.sin(q_left) fk["right_toe"] = Point.Point(0, 0, 0) fk["right_toe"].x = fk["right_ankle"].x - 0.8 * ( 8.0 / 100.0) * 1.57 * np.cos(q_right) fk["right_toe"].y = fk["right_ankle"].y - 0.8 * ( 8.0 / 100.0) * 1.57 * np.cos(q_right) fk["right_toe"].z = fk["right_ankle"].z - 0.05 + 0.8 * ( 8.0 / 100.0) * self._height * np.sin(q_right) fk["right_heel"] = Point.Point(0, 0, 0) fk["right_heel"].x = fk["right_ankle"].x + 0.2 * ( 8.0 / 100.0) * self._height * np.cos(q_right) fk["right_heel"].y = fk["right_ankle"].y + 0.2 * ( 8.0 / 100.0) * self._height * np.cos(q_right) fk["right_heel"].z = fk["right_ankle"].z - 0.05 + 0.2 * ( 8.0 / 100.0) * self._height * np.sin(q_right) return fk
def cb_joint_state(self, data): self.joint_names = sort_data(data.name) self.q = np.array(sort_data(data.position)) rbdl.CalcBodyToBaseCoordinates(self.model, self.q, 0, self.X, False) rbdl.CalcBodySpatialJacobian(self.model, self.q, 0, self.X, self.J, False) rbdl.UpdateKinematics(self.model, self.q, self.qd, self.qdd) self.calculate_torque() self.calculate_acceleration()
def test_CoordinateTransformBodyBase(self): """ Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates give the right results. """ q = np.random.rand(self.model.q_size) point_local = np.array([1., 2., 3.]) point_base = rbdl.CalcBodyToBaseCoordinates(self.model, q, self.body_3, point_local) point_local_2 = rbdl.CalcBaseToBodyCoordinates(self.model, q, self.body_3, point_base) assert_almost_equal(point_local, point_local_2)
def fk(self, body_name, q=None, body_offset=np.zeros(3), update_kinematics=True, rotation_rep='quat'): """ Forward kinematics for a joint configuration. :param body_name: Name of the body :param q: Joint configuration. If it is not specified, the current q value of the model is used. :param body_offset: Body offset. :param update_kinematics: Update model with the joint configuration. :param rotation_rep: Representation used to represent the rotation of the body: 'quat' or 'rpy' :return: Pose of the requested body. """ if body_name == 'Waist' and not self.floating_base: body_name = 'ROOT' if q is None: q = self.q body_id = self.model.GetBodyId(body_name) pos = rbdl.CalcBodyToBaseCoordinates( self.model, q, body_id, body_offset, update_kinematics=update_kinematics ) rot = rbdl.CalcBodyWorldOrientation( self.model, q, body_id, update_kinematics=update_kinematics ).T if rotation_rep == 'rpy': orient = np.array( tf_transformations.euler_from_matrix( homogeneous_matrix(rot=rot), axes='sxyz' ) ) elif rotation_rep == 'quat': orient = tf_transformations.quaternion_from_matrix( homogeneous_matrix(rot=rot) ) else: raise TypeError("Wrong requested rotation representation: %s" % rotation_rep) return np.concatenate((orient, pos))
def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)): """ Return the pose of the specified link with respect to the other given link. Args: link (int, str): unique link id, or name. wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, and if -1 wrt to the base. point (np.array[float[3]]): position of the point in link's local frame. Returns: np.array[float[7]]: pose (position and quaternion expressed as [x,y,z,w]) """ link = self.get_link_id(link) point = np.asarray(point) position = rbdl.CalcBodyToBaseCoordinates(self.model, self._q, link, point, update_kinematics=False) orientation = rbdl.CalcBodyWorldOrientation(self.model, self._q, link, update_kinematics=False) orientation = get_quaternion_from_matrix(orientation) return np.concatenate((position, orientation))
def fk(model, body_name, q=None, body_offset=np.zeros(3), update_kinematics=True, rotation_rep='quat'): """ :param model: :param body_name: :param q: :param body_offset: :param update_kinematics: :param rotation_rep: :return: """ body_name = body_name.encode() if q is None: q = np.zeros(model.q_size) body_id = model.GetBodyId(body_name) pos = rbdl.CalcBodyToBaseCoordinates(model, q, body_id, body_offset, update_kinematics=update_kinematics) rot = rbdl.CalcBodyWorldOrientation(model, q, body_id, update_kinematics=update_kinematics).T if rotation_rep == 'quat': orient = tf_transformations.quaternion_from_matrix( homogeneous_matrix(rot=rot)) elif rotation_rep == 'rpy': orient = np.array( tf_transformations.euler_from_matrix(homogeneous_matrix(rot=rot), axes='sxyz')) else: return AttributeError( "Wrong rotation representation %s. Only 'rpy' and 'quat' available." % rotation_rep) return np.concatenate((orient, pos))
def __init__(self, urdf_file, floating_base=False): self.floating_base = floating_base self.model = rbdl.loadModel(urdf_file, verbose=False, floating_base=floating_base) self.q = np.zeros(self.model.q_size) self.qdot = np.zeros(self.model.qdot_size) self.qddot = np.zeros(self.model.qdot_size) self.tau = np.zeros(self.model.qdot_size) self.q_size = self.q.shape[0] self.qdot_size = self.qdot.shape[0] self.qddot_size = self.qddot.shape[0] # Update model self.update() floating_base_body_id = 0 self.fb_origin_offset = rbdl.CalcBodyToBaseCoordinates( self.model, self.q * 0, floating_base_body_id, np.zeros(3), False )
def fwd_kinematics(model, q, body): size = len(q) q = q.reshape(size, ) point_local = np.array([0.0, 0.0, 0.0]) end_pos = rbdl.CalcBodyToBaseCoordinates(model, q, body, point_local) return end_pos
print(xtrans) # 三杆模型,第一个body,绕ry转动 body_1 = model.AppendBody(rbdl.SpatialTransform(), joint_rot_y, body) body_2 = model.AppendBody(xtrans, joint_rot_y, body) body_3 = model.AppendBody(xtrans, joint_rot_y, body) # 获取模型中变量 q = np.zeros(model.q_size) qdot = np.zeros(model.qdot_size) qddot = np.zeros(model.qdot_size) tau = np.zeros(model.qdot_size) # 设置当前角度 q[0] = 1.3 q[1] = -0.5 q[2] = 3.2 # 计算运动学关系 point_local = np.array([1., 2., 3.]) # 一个局部坐标值 point_base = rbdl.CalcBodyToBaseCoordinates( model, q, body_3, point_local) # 在body3的局部坐标系下的的点在全局中的坐标 point_local_2 = rbdl.CalcBaseToBodyCoordinates(model, q, body_3, point_base) # 反过来 # 前向动力学 # 这个破模型当然计算得到的加速度全都是零了,真是傻了 rbdl.ForwardDynamics(model, q, qdot, tau, qddot) print("qddot = " + str(qddot.transpose())) G = np.zeros([3, model.qdot_size]) rbdl.CalcPointJacobian(model, q, body_3, point_local, G) print("G = \n" + str(G))
def get_end_effector_pos(q): point_local = np.array([0.0, 0.0, 0.0]) end_pos = rbdl.CalcBodyToBaseCoordinates(model, q, body_7, point_local) return end_pos
# Construct the model body_1 = model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, body) body_2 = model.AppendBody (xtrans, joint_rot_y, body) body_3 = model.AppendBody (xtrans, joint_rot_y, body) # Create numpy arrays for the state q = np.zeros (model.q_size) qdot = np.zeros (model.qdot_size) qddot = np.zeros (model.qdot_size) tau = np.zeros (model.qdot_size) # Modify the state q[0] = 1.3 q[1] = -0.5 q[2] = 3.2 # Transform coordinates from local to global coordinates point_local = np.array([1., 2., 3.]) point_base = rbdl.CalcBodyToBaseCoordinates (model, q, body_3, point_local) point_local_2 = rbdl.CalcBaseToBodyCoordinates (model, q, body_3, point_base) # Perform forward dynamics and print the result rbdl.ForwardDynamics (model, q, qdot, tau, qddot) print ("qddot = " + str(qddot.transpose())) # Compute and print the jacobian (note: the output parameter # of the Jacobian G must have proper size!) G = np.zeros([3,model.qdot_size]) rbdl.CalcPointJacobian (model, q, body_3, point_local, G) print ("G = \n" + str(G))
def jointStateCallback(data): #global variables shared between functions global pose_des global vel_des global q global q_old global qd J = np.zeros((6, 7)) h = 0.01 #dt K = (2 / h) * np.eye(6) #Stability for < 2/h EE_LOCAL_POS = np.array((0.0, 0.0, 0.045)) #Local offset k = 0.001 #Damped least square method I = np.eye(6) ## JointState msg ## q = [gripper_left_joint, gripper_right_joint, iiwa_joint_1, iiwa_joint_2, iiwa_joint_3, iiwa_joint_4, iiwa_joint_5, iiwa_joint_6, iiwa_joint_7] q = data.position[2:] q = np.asarray(q) q_old = q qd = data.velocity[2:] qd = np.asarray(qd) #End-Effector Position x = rbdl.CalcBodyToBaseCoordinates(model, q, 7, EE_LOCAL_POS, True) #x,y,z #Orientation R = rbdl.CalcBodyWorldOrientation(model, q, 7, True).T #From rotation matrix to quaternion quat = quaternion.from_rotation_matrix(R) #Input as euler angles for orientation quat_des = quaternion.from_euler_angles(pose_des[0], pose_des[1], pose_des[2]) #Angular error err_ang = quat_des * quat.conjugate() err_ang = quaternion.as_float_array(err_ang) #Linear error err_lin = np.array( ([pose_des[3] - x[0], pose_des[4] - x[1], pose_des[5] - x[2]])) #Total Error err = np.array(([ err_ang[1], err_ang[2], err_ang[3], err_lin[0], err_lin[1], err_lin[2] ])) # Jacobian (angular-linear) rbdl.CalcPointJacobian6D(model, q, 7, EE_LOCAL_POS, J) # Jacobian Inverse (Damped) J_inv = J.T.dot(inv(J.dot(J.transpose()) + (k**2) * I)) #CLIK: Closed Loop Inverse Kinematics q = q_old + h * J_inv.dot(vel_des + K.dot(err)) qd = J_inv.dot(vel_des + K.dot(err)) #rqt_plot for error pub_error = rospy.Publisher('/iiwa/iiwa_position_controller/error', Float64, queue_size=10) pub_error.publish(norm(err)) #POSITION CONTROLLER pub = rospy.Publisher('/iiwa/iiwa_position_controller/command', JointTrajectory, queue_size=10) #JointTrajectory msg generation joints_str = JointTrajectory() joints_str.header = Header() joints_str.header.stamp = rospy.Time.now() joints_str.joint_names = [ 'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7' ] #I'll use a single waypoint with the q[] values calculated with CLIK point = JointTrajectoryPoint() point.positions = [q[0], q[1], q[2], q[3], q[4], q[5], q[6]] #point.velocities = [ qd[0], qd[1], qd[2], qd[3], qd[4], qd[5], qd[6] ] point.time_from_start = rospy.Duration(1) joints_str.points.append(point) pub.publish(joints_str) #Gripper Controller pub_left = rospy.Publisher( '/iiwa/gripper_left_position_controller/command', Float64, queue_size=10) pub_right = rospy.Publisher( '/iiwa/gripper_right_position_controller/command', Float64, queue_size=10) #Open Position pub_left.publish(0) pub_right.publish(0) rate.sleep() q_old = q
cs.Bind(model) q = np.zeros(model.q_size) qdot = np.zeros(model.qdot_size) qddot = np.zeros(model.qdot_size) tau = np.zeros(model.qdot_size) print("shape of q:", np.shape(q)) print("Size of q:", np.size(q)) # Testing the 4-bar linkage in the 0 degree angle for all the joints q[0] = 0. q[1] = 0. q[2] = 0. q[3] = 0. q[4] = 0. pos1 = rbdl.CalcBodyToBaseCoordinates(model, q, idB2, Xp.r) pos2 = rbdl.CalcBodyToBaseCoordinates(model, q, idB5, Xs.r) print 'Point Location wrt base: ', pos1 print 'Point Location wrt base: ', pos2 # Testing the 4-bar linkage in the with some non-zero angle q[0] = np.pi q[1] = -np.pi q[2] = np.pi - q[0] q[3] = -q[1] q[4] = 0. pos1 = rbdl.CalcBodyToBaseCoordinates(model, q, idB2, Xp.r) pos2 = rbdl.CalcBodyToBaseCoordinates(model, q, idB5, Xs.r) print 'Point Location wrt base: ', pos1 print 'Point Location wrt base: ', pos2 # Testing the 4-bar linkage in the with some non-zero angle q_val = np.pi / 4
# print 'body', body_1 # print 'Inertia', body.mInertia # print 'trans', trans q = np.zeros(model.q_size) qdot = np.zeros(model.qdot_size) qddot = np.zeros(model.qdot_size) tau = np.zeros(model.qdot_size) print(np.shape(q)) print(np.size(q)) q[0] = 0 #-105*3.14/180# 3.14 # # Giving an arbitrary location described in the local frame and printing it's # location wrt the world frame COM_L1 = np.array([0.0, 0.0, 0.0]) COM_L1_base = rbdl.CalcBodyToBaseCoordinates(model, q, body_1, COM_L1) print 'COM_L1_base: ', COM_L1_base # Giving an arbitrary location described in the local frame and printing it's # location wrt the world frame COM_L3 = np.array([0.0, 0.0, 0.0]) COM_L3_base = rbdl.CalcBodyToBaseCoordinates(model, q, body_3, COM_L3) print 'COM_L3_base: ', COM_L3_base rbdl.InverseDynamics(model, q, qdot, qddot, tau) print 'G: ', tau def get_G(q_): q_ = np.asarray(q_)
def draw_model(self, bbmdl, q): mdl = bbmdl.model gl.glColor3f(0,1,1) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.zero) draw_line(pt0[0:2], (0,0)) gl.glColor3f(0,1,0) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.zero, update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.unit_y, update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) gl.glColor3f(0,0,1) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.roller, self.zero, update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.zero, update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) gl.glColor3f(1,.5,0) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, np.array([-self.l_board/2,self.r_roller,0]), update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, np.array([self.l_board/2,self.r_roller,0]), update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) gl.glColor3f(1,0,0) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.zero, update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.unit_y, update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.board, self.zero, update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body, self.zero, update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) gl.glColor3f(0,1,0) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body, self.zero, update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body, np.asarray([0,.1,0]), update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) try: gl.glColor3f(0,0,1) pt0 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body2, self.zero, update_kinematics = False) pt1 = rbdl.CalcBodyToBaseCoordinates(mdl, q, bbmdl.body2, np.asarray([0,-self.h_body + self.r_roller + .05,0]), update_kinematics = False) draw_line(pt0[0:2], pt1[0:2]) except AttributeError: pass