DMP modeling and planning test on UR5 robot

README.md

@@ -6,6 +6,9 @@ This code is a Python implementation of DMPs by Chauby (Email: chaubyZou@163.com
 
 The corresponding tutorial can be found at：
 
+- Zhihu：
+- Wechat：
+
 
 
 
@@ -23,9 +26,13 @@ Requirements:
 
 
 
+For simulation on CoppeliaSim, you need to install the CoppeliaSim 4.2 or later version.
+
+
+
 ---
 
-## Canonical system test
+## Canonical System
 
 Canonical system with different parameters:
 
@@ -35,7 +42,7 @@ Canonical system with different parameters:
 
 ----
 
-## Discrete DMP test
+## Discrete DMP
 
 The DMP model is used to model and reproduce sine and cosine trajectories with a limited time.
 
@@ -47,7 +54,7 @@ The solid curves represent the demonstrated trajectories, the dashed curves repr
 
 ---
 
-## Rhythmic DMP test
+## Rhythmic DMP
 
 For demonstrated trajecotry with only one dimension.
 
@@ -59,14 +66,46 @@ For demonstrated trajecotry with two dimensions.
 
 ---
 
-# Simulation on CoppeliaSim (V-REP)
+# Simulation on UR5 robot in CoppeliaSim (V-REP)
+
+## User guide:
+
+1. Open the CoppeliaSim project file in the folder "coppeliasim";
+2. Click the "run" button of the CoppeliaSim to run the project file;
+3. Run python script "demo_rhythmic_DMP_UR5.py" or "demo_discrete_DMP_UR5.py".
+
+
 
 ## Discrete DMP
 
+The position of the end-effector are set by the code, and the inverse kinematics calculation model in CoppeliaSim are used to calculate the joint angles of UR5.
+
+![DMP_UR5_discrete](README.assets/DMP_UR5_discrete.gif)
+
+
+
+The corresponding positions for the end-effector of UR5 are shown as follows:
+
+Randomly modiy goal positions of the reproduced trajectory:
+
+![DMP_discrete_different_goals](README.assets/DMP_discrete_different_goals.png)
+
+Randomly modify initial and goal positions of the reproduced trajectory:
+
+![DMP_discrete_different_initial_goals](README.assets/DMP_discrete_different_initial_goals.png)
+
 
 
 ## Rhythmic DMP
 
+Three joints of the UR5 robot are under control.
+
+![DMP_UR5_rhythmic](README.assets/DMP_UR5_rhythmic.gif)
+
+The corresponding joint angles are show as follows:
+
+<img src="README.assets/UR5_rhythmic.png" alt="UR5_rhythmic" style="zoom:80%;" />
+
 
 
 ---
@@ -77,4 +116,5 @@ The reference paper can be found at the folder named 'paper', and can also be do
 
 - [2002 Stefan Schaal](http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.142.3886)
 
-- [2013 Auke Ijspeert](http://www-clmc.usc.edu/publications/I/ijspeert-NC2013.pdf)
+- [2013 Auke Ijspeert](http://www-clmc.usc.edu/publications/I/ijspeert-NC2013.pdf)
+

code/UR5/UR5SimModel.py

@@ -0,0 +1,133 @@
+# -*- coding: utf-8 -*-
+import sys
+sys.path.append('./VREP_RemoteAPIs')
+import sim as vrep_sim
+
+# UR5 simulation model in CoppeliaSim
+class UR5SimModel():
+    def __init__(self, name='UR5'):
+        """
+        :param: name: string
+            name of objective
+        """
+        super(self.__class__, self).__init__()
+        self.name = name
+        self.client_ID = None
+
+        # joint handle
+        self.joint_handle = [-1, -1, -1, -1, -1, -1]
+
+        # joint angles
+        self.joint_angle = [0, 0, 0, 0, 0, 0]
+
+    def initializeSimModel(self, client_ID):
+        try:
+            print ('Connected to remote API server')
+            client_ID != -1
+        except:
+            print ('Failed connecting to remote API server')
+
+        self.client_ID = client_ID
+
+        for i in range(6):
+            return_code, self.joint_handle[i] = vrep_sim.simxGetObjectHandle(client_ID, 'UR5_joint' + str(i+1), vrep_sim.simx_opmode_blocking)
+            if (return_code == vrep_sim.simx_return_ok):
+                print('get object joint handle ' + str(i+1) + ' ok.')
+
+        # Get information from VREP for the first time
+        for i in range(6):
+            return_code, q = vrep_sim.simxGetJointPosition(self.client_ID, self.joint_handle[i], vrep_sim.simx_opmode_streaming)
+
+        # Set the initialized position for each joint
+        for i in range(6):
+            vrep_sim.simxSetJointTargetPosition(self.client_ID, self.joint_handle[i], 0, vrep_sim.simx_opmode_streaming)
+
+
+    def getJointAngle(self, joint_name):
+        """
+        :param: joint_name: str
+            the joint name: can be UR5_joint1 ~ UR5_joint6
+        """
+        for i in range(6):
+            if joint_name == 'UR5_joint' + str(i+1):
+                return_code, q = vrep_sim.simxGetJointPosition(self.client_ID, self.joint_handle[i], vrep_sim.simx_opmode_buffer)
+                return q
+
+        # can not find the joint
+        print('Error: joint name: \' ' + joint_name + '\' can not be recognized.')
+        return 0
+
+    def getAllJointAngles(self):
+        q = [0, 0, 0, 0, 0, 0]
+        for i in range(6):
+            return_code, q[i] = vrep_sim.simxGetJointPosition(self.client_ID, self.joint_handle[i], vrep_sim.simx_opmode_buffer)
+
+        return q
+
+    def setJointAngle(self, joint_name, q):
+        """
+        :param: joint_name: str
+            the joint name: can be UR5_joint1 ~ UR5_joint6
+        :param: q: float array of size 6 x 1
+            the desired joint angle for all joints
+        """
+
+        for i in range(6):
+            if joint_name == 'UR5_joint' + str(i+1):
+                vrep_sim.simxSetJointTargetPosition(self.client_ID, self.joint_handle[i], q, vrep_sim.simx_opmode_streaming)
+                return
+
+        # can not find the joint
+        print('Error: joint name: \' ' + joint_name + '\' can not be recognized.')
+        return 0
+
+#%% test code
+if __name__ == "__main__":
+    import time
+    import numpy as np
+
+    print ('Program started')
+
+    # ------------------------------- Connect to VREP (CoppeliaSim) ------------------------------- 
+    vrep_sim.simxFinish(-1) # just in case, close all opened connections
+    while True:
+        client_ID = vrep_sim.simxStart('127.0.0.1', 19999, True, False, 5000, 5) # Connect to CoppeliaSim
+        if client_ID > -1: # connected
+            print('Connect to remote API server.')
+            break
+        else:
+            print('Failed connecting to remote API server! Try it again ...')
+
+    # Pause the simulation
+    # res = vrep_sim.simxPauseSimulation(client_ID, vrep_sim.simx_opmode_blocking)
+
+    delta_t = 0.005 # simulation time step
+    # Set the simulation step size for VREP
+    vrep_sim.simxSetFloatingParameter(client_ID, vrep_sim.sim_floatparam_simulation_time_step, delta_t, vrep_sim.simx_opmode_oneshot)
+    # Open synchronous mode
+    vrep_sim.simxSynchronous(client_ID, True) 
+    # Start simulation
+    vrep_sim.simxStartSimulation(client_ID, vrep_sim.simx_opmode_oneshot)
+
+    # ------------------------------- Initialize simulation model ------------------------------- 
+
+    UR5_sim_model = UR5SimModel()
+    UR5_sim_model.initializeSimModel(client_ID)
+    time.sleep(0.1)
+    desired_q = [0, 0, 0, 0, 0, 0]
+
+    print("Main loop is begining ...")
+    t_max = 20
+    for t in np.linspace(0, t_max, t_max*100):
+        q = UR5_sim_model.getAllJointAngles()
+        # print(q)
+
+        desired_q = [0, 0, np.sin(t), 1.5*np.sin(t), 0.8*np.sin(t), 0]
+        for i in range(6):
+            UR5_sim_model.setJointAngle('UR5_joint'+ str(i+1), desired_q[i])
+
+        vrep_sim.simxSynchronousTrigger(client_ID)  # trigger one simulation step
+
+    vrep_sim.simxStopSimulation(client_ID, vrep_sim.simx_opmode_blocking) # stop the simulation
+    vrep_sim.simxFinish(-1)  # Close the connection
+    print('Program terminated')

code/UR5/VREP_RemoteAPIs/complexCommandTest.py

@@ -0,0 +1,59 @@
+# This example illustrates how to execute complex commands from
+# a remote API client. You can also use a similar construct for
+# commands that are not directly supported by the remote API.
+#
+# Load the demo scene 'remoteApiCommandServerExample.ttt' in CoppeliaSim, then 
+# start the simulation and run this program.
+#
+# IMPORTANT: for each successful call to simxStart, there
+# should be a corresponding call to simxFinish at the end!
+
+try:
+    import sim
+except:
+    print ('--------------------------------------------------------------')
+    print ('"sim.py" could not be imported. This means very probably that')
+    print ('either "sim.py" or the remoteApi library could not be found.')
+    print ('Make sure both are in the same folder as this file,')
+    print ('or appropriately adjust the file "sim.py"')
+    print ('--------------------------------------------------------------')
+    print ('')
+
+import sys
+import ctypes
+print ('Program started')
+sim.simxFinish(-1) # just in case, close all opened connections
+clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
+if clientID!=-1:
+    print ('Connected to remote API server')
+
+    # 1. First send a command to display a specific message in a dialog box:
+    emptyBuff = bytearray()
+    res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayText_function',[],[],['Hello world!'],emptyBuff,sim.simx_opmode_blocking)
+    if res==sim.simx_return_ok:
+        print ('Return string: ',retStrings[0]) # display the reply from CoppeliaSim (in this case, just a string)
+    else:
+        print ('Remote function call failed')
+
+    # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
+    res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'createDummy_function',[],[0.1,0.2,0.3],['MyDummyName'],emptyBuff,sim.simx_opmode_blocking)
+    if res==sim.simx_return_ok:
+        print ('Dummy handle: ',retInts[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy)
+    else:
+        print ('Remote function call failed')
+
+    # 3. Now send a code string to execute some random functions:
+    code="local octreeHandle=simCreateOctree(0.5,0,1)\n" \
+    "simInsertVoxelsIntoOctree(octreeHandle,0,{0.1,0.1,0.1},{255,0,255})\n" \
+    "return 'done'"
+    res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,"remoteApiCommandServer",sim.sim_scripttype_childscript,'executeCode_function',[],[],[code],emptyBuff,sim.simx_opmode_blocking)
+    if res==sim.simx_return_ok:
+        print ('Code execution returned: ',retStrings[0])
+    else:
+        print ('Remote function call failed')
+
+    # Now close the connection to CoppeliaSim:
+    sim.simxFinish(clientID)
+else:
+    print ('Failed connecting to remote API server')
+print ('Program ended')