<jupyter_start><jupyter_text>DQ Representation for Cyton Arm from DH<jupyter_code>from DQ import *
from DQ_kinematics import *
import numpy as np
import scipy.linalg as LA
from math import pi

def DQ_CYTON():
    '''
    DQ Representation for COMAU Arm
    '''
    
    cyton_DH_theta=  np.array([0, 0, 0, 0, pi, pi/2, 0])
    cyton_DH_d =     np.array([0.0379, -0.0046, 0.145, -0.011, 0.175, 0.0074, 0.0])
    cyton_DH_a =     np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0677])
    cyton_DH_alpha = np.array([0, pi/2, -pi/2, pi/2, -pi/2, pi/2, pi/2])
    cyton_dummy =    np.array([0,0,0,0,0,0,0])

    cyton_DH_matrix = np.array([cyton_DH_theta,cyton_DH_d,cyton_DH_a,cyton_DH_alpha,cyton_dummy])

    cyton = DQ_kinematics(cyton_DH_matrix, 'modified')

    return cyton<jupyter_output><empty_output><jupyter_text>Open Loop Control Simulation<jupyter_code>robotarm_kine = DQ_CYTON();

# Basic definitions for the simulation
theta  = np.array([0,0,-pi/2,0,pi/2,0,0])
thetad = np.array([pi/2,0,-pi/2,0,pi/2,0,0])

# Desired end-effector pose
xd = robotarm_kine.fkm(thetad);

error = 1;
epsilon = 0.001;
K = 0.5;

while LA.norm(error) > epsilon:
    x = robotarm_kine.fkm(theta);
    J = robotarm_kine.jacobian(theta);
    error = vec8(xd-x);
    theta = theta + np.dot(np.dot(LA.pinv(J),K),error);
    print LA.norm(error)<jupyter_output>0.782529172168
1.3119865458
1.08703292083
0.675011182588
0.38063384537
0.201940598604
0.107061773686
0.0574885111265
0.031324435465
0.0174591895213
0.0101612863653
0.00643709787186
0.00474493952727
0.00436589985638
0.00521978010093
0.00782097500285
0.00868096332387
0.00481146296309
0.00246832919243
0.00124439555779
0.000623597051812<jupyter_text>Open Loop Control Using ROS<jupyter_code>import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float32MultiArray
import numpy as np

robotarm_kine = DQ_CYTON();

# Basic definitions for the execution
theta  = np.array([0,0,-pi/2,0,pi/2,0,0])
thetad = np.array([pi/2,0,-pi/2,0,pi/2,0,0])

# Desired end-effector pose
xd = robotarm_kine.fkm(thetad);

error = 1;
epsilon = 0.001;
K = 0.5;

rospy.init_node('cyton_feedback',anonymous=False)
pub = rospy.Publisher('/Cyton/jointCmd', Float32MultiArray, queue_size=10)
rate = rospy.Rate(10) # 10hz

while LA.norm(error) > epsilon:
    x = robotarm_kine.fkm(theta);
    J = robotarm_kine.jacobian(theta);
    error = vec8(xd-x);
    theta = theta + np.dot(np.dot(LA.pinv(J),K),error);
    
    cmd_msg = Float32MultiArray()
    cmd_msg.data = theta
    pub.publish(cmd_msg)
    
    rospy.loginfo(LA.norm(error))
    rate.sleep()<jupyter_output>[INFO] [WallTime: 1502833880.133564] 0.782529172168
[INFO] [WallTime: 1502833880.233439] 1.3119865458
[INFO] [WallTime: 1502833880.333682] 1.08703292083
[INFO] [WallTime: 1502833880.433269] 0.675011182588
[INFO] [WallTime: 1502833880.533491] 0.38063384537
[INFO] [WallTime: 1502833880.634394] 0.201940598604
[INFO] [WallTime: 1502833880.733675] 0.107061773686
[INFO] [WallTime: 1502833880.835532] 0.0574885111265
[INFO] [WallTime: 1502833880.934048] 0.031324435465
[INFO] [WallTime: 1502833881.033740] 0.0174591895213
[INFO] [WallTime: 1502833881.134059] 0.0101612863653
[INFO] [WallTime: 1502833881.233883] 0.00643709787186
[INFO] [WallTime: 1502833881.333772] 0.00474493952727
[INFO] [WallTime: 1502833881.433344] 0.00436589985638
[INFO] [WallTime: 1502833881.533679] 0.00521978010093
[INFO] [WallTime: 1502833881.633497] 0.00782097500285
[INFO] [WallTime: 1502833881.733486] 0.00868096332387
[INFO] [WallTime: 1502833881.833478] 0.00481146296309
[INFO] [WallTime: 1502833881.933594] 0.002468329192[...]
{'fraction_non_alphanumeric': 0.141467481934408, 'fraction_numerical': 0.27209560867148413, 'mean_word_length': 4.938943894389439, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 13, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '654747', 'license': 'MIT', 'n_tokens_mistral': 2212, 'n_tokens_neox': 1608, 'n_words': 271, 'path': 'notes/KinematicsControl_Cyton.ipynb', 'repo_name': 'akafael/dual-quaternions', 'size': '3598'}